1d2912cb1SThomas Gleixner // SPDX-License-Identifier: GPL-2.0-only
26ae0a628SYotam Gigi /*
36ae0a628SYotam Gigi * net/psample/psample.c - Netlink channel for packet sampling
46ae0a628SYotam Gigi * Copyright (c) 2017 Yotam Gigi <yotamg@mellanox.com>
56ae0a628SYotam Gigi */
66ae0a628SYotam Gigi
76ae0a628SYotam Gigi #include <linux/types.h>
86ae0a628SYotam Gigi #include <linux/kernel.h>
96ae0a628SYotam Gigi #include <linux/skbuff.h>
106ae0a628SYotam Gigi #include <linux/module.h>
1107e1a580SIdo Schimmel #include <linux/timekeeping.h>
126ae0a628SYotam Gigi #include <net/net_namespace.h>
136ae0a628SYotam Gigi #include <net/sock.h>
146ae0a628SYotam Gigi #include <net/netlink.h>
156ae0a628SYotam Gigi #include <net/genetlink.h>
166ae0a628SYotam Gigi #include <net/psample.h>
176ae0a628SYotam Gigi #include <linux/spinlock.h>
18d8bed686SChris Mi #include <net/ip_tunnels.h>
19d8bed686SChris Mi #include <net/dst_metadata.h>
206ae0a628SYotam Gigi
216ae0a628SYotam Gigi #define PSAMPLE_MAX_PACKET_SIZE 0xffff
226ae0a628SYotam Gigi
236ae0a628SYotam Gigi static LIST_HEAD(psample_groups_list);
246ae0a628SYotam Gigi static DEFINE_SPINLOCK(psample_groups_lock);
256ae0a628SYotam Gigi
266ae0a628SYotam Gigi /* multicast groups */
276ae0a628SYotam Gigi enum psample_nl_multicast_groups {
286ae0a628SYotam Gigi PSAMPLE_NL_MCGRP_CONFIG,
296ae0a628SYotam Gigi PSAMPLE_NL_MCGRP_SAMPLE,
306ae0a628SYotam Gigi };
316ae0a628SYotam Gigi
326ae0a628SYotam Gigi static const struct genl_multicast_group psample_nl_mcgrps[] = {
336ae0a628SYotam Gigi [PSAMPLE_NL_MCGRP_CONFIG] = { .name = PSAMPLE_NL_MCGRP_CONFIG_NAME },
34*580c80b6SIdo Schimmel [PSAMPLE_NL_MCGRP_SAMPLE] = { .name = PSAMPLE_NL_MCGRP_SAMPLE_NAME,
35*580c80b6SIdo Schimmel .flags = GENL_UNS_ADMIN_PERM },
366ae0a628SYotam Gigi };
376ae0a628SYotam Gigi
386ae0a628SYotam Gigi static struct genl_family psample_nl_family __ro_after_init;
396ae0a628SYotam Gigi
psample_group_nl_fill(struct sk_buff * msg,struct psample_group * group,enum psample_command cmd,u32 portid,u32 seq,int flags)406ae0a628SYotam Gigi static int psample_group_nl_fill(struct sk_buff *msg,
416ae0a628SYotam Gigi struct psample_group *group,
426ae0a628SYotam Gigi enum psample_command cmd, u32 portid, u32 seq,
436ae0a628SYotam Gigi int flags)
446ae0a628SYotam Gigi {
456ae0a628SYotam Gigi void *hdr;
466ae0a628SYotam Gigi int ret;
476ae0a628SYotam Gigi
486ae0a628SYotam Gigi hdr = genlmsg_put(msg, portid, seq, &psample_nl_family, flags, cmd);
496ae0a628SYotam Gigi if (!hdr)
506ae0a628SYotam Gigi return -EMSGSIZE;
516ae0a628SYotam Gigi
526ae0a628SYotam Gigi ret = nla_put_u32(msg, PSAMPLE_ATTR_SAMPLE_GROUP, group->group_num);
536ae0a628SYotam Gigi if (ret < 0)
546ae0a628SYotam Gigi goto error;
556ae0a628SYotam Gigi
566ae0a628SYotam Gigi ret = nla_put_u32(msg, PSAMPLE_ATTR_GROUP_REFCOUNT, group->refcount);
576ae0a628SYotam Gigi if (ret < 0)
586ae0a628SYotam Gigi goto error;
596ae0a628SYotam Gigi
606ae0a628SYotam Gigi ret = nla_put_u32(msg, PSAMPLE_ATTR_GROUP_SEQ, group->seq);
616ae0a628SYotam Gigi if (ret < 0)
626ae0a628SYotam Gigi goto error;
636ae0a628SYotam Gigi
646ae0a628SYotam Gigi genlmsg_end(msg, hdr);
656ae0a628SYotam Gigi return 0;
666ae0a628SYotam Gigi
676ae0a628SYotam Gigi error:
686ae0a628SYotam Gigi genlmsg_cancel(msg, hdr);
696ae0a628SYotam Gigi return -EMSGSIZE;
706ae0a628SYotam Gigi }
716ae0a628SYotam Gigi
psample_nl_cmd_get_group_dumpit(struct sk_buff * msg,struct netlink_callback * cb)726ae0a628SYotam Gigi static int psample_nl_cmd_get_group_dumpit(struct sk_buff *msg,
736ae0a628SYotam Gigi struct netlink_callback *cb)
746ae0a628SYotam Gigi {
756ae0a628SYotam Gigi struct psample_group *group;
766ae0a628SYotam Gigi int start = cb->args[0];
776ae0a628SYotam Gigi int idx = 0;
786ae0a628SYotam Gigi int err;
796ae0a628SYotam Gigi
804a5da47dSVlad Buslov spin_lock_bh(&psample_groups_lock);
816ae0a628SYotam Gigi list_for_each_entry(group, &psample_groups_list, list) {
826ae0a628SYotam Gigi if (!net_eq(group->net, sock_net(msg->sk)))
836ae0a628SYotam Gigi continue;
846ae0a628SYotam Gigi if (idx < start) {
856ae0a628SYotam Gigi idx++;
866ae0a628SYotam Gigi continue;
876ae0a628SYotam Gigi }
886ae0a628SYotam Gigi err = psample_group_nl_fill(msg, group, PSAMPLE_CMD_NEW_GROUP,
896ae0a628SYotam Gigi NETLINK_CB(cb->skb).portid,
906ae0a628SYotam Gigi cb->nlh->nlmsg_seq, NLM_F_MULTI);
916ae0a628SYotam Gigi if (err)
926ae0a628SYotam Gigi break;
936ae0a628SYotam Gigi idx++;
946ae0a628SYotam Gigi }
956ae0a628SYotam Gigi
964a5da47dSVlad Buslov spin_unlock_bh(&psample_groups_lock);
976ae0a628SYotam Gigi cb->args[0] = idx;
986ae0a628SYotam Gigi return msg->len;
996ae0a628SYotam Gigi }
1006ae0a628SYotam Gigi
10166a9b928SJakub Kicinski static const struct genl_small_ops psample_nl_ops[] = {
1026ae0a628SYotam Gigi {
1036ae0a628SYotam Gigi .cmd = PSAMPLE_CMD_GET_GROUP,
104ef6243acSJohannes Berg .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
1056ae0a628SYotam Gigi .dumpit = psample_nl_cmd_get_group_dumpit,
1066ae0a628SYotam Gigi /* can be retrieved by unprivileged users */
1076ae0a628SYotam Gigi }
1086ae0a628SYotam Gigi };
1096ae0a628SYotam Gigi
1106ae0a628SYotam Gigi static struct genl_family psample_nl_family __ro_after_init = {
1116ae0a628SYotam Gigi .name = PSAMPLE_GENL_NAME,
1126ae0a628SYotam Gigi .version = PSAMPLE_GENL_VERSION,
1136ae0a628SYotam Gigi .maxattr = PSAMPLE_ATTR_MAX,
1146ae0a628SYotam Gigi .netnsok = true,
1156ae0a628SYotam Gigi .module = THIS_MODULE,
1166ae0a628SYotam Gigi .mcgrps = psample_nl_mcgrps,
11766a9b928SJakub Kicinski .small_ops = psample_nl_ops,
11866a9b928SJakub Kicinski .n_small_ops = ARRAY_SIZE(psample_nl_ops),
1199c5d03d3SJakub Kicinski .resv_start_op = PSAMPLE_CMD_GET_GROUP + 1,
1206ae0a628SYotam Gigi .n_mcgrps = ARRAY_SIZE(psample_nl_mcgrps),
1216ae0a628SYotam Gigi };
1226ae0a628SYotam Gigi
psample_group_notify(struct psample_group * group,enum psample_command cmd)1236ae0a628SYotam Gigi static void psample_group_notify(struct psample_group *group,
1246ae0a628SYotam Gigi enum psample_command cmd)
1256ae0a628SYotam Gigi {
1266ae0a628SYotam Gigi struct sk_buff *msg;
1276ae0a628SYotam Gigi int err;
1286ae0a628SYotam Gigi
1296ae0a628SYotam Gigi msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);
1306ae0a628SYotam Gigi if (!msg)
1316ae0a628SYotam Gigi return;
1326ae0a628SYotam Gigi
1336ae0a628SYotam Gigi err = psample_group_nl_fill(msg, group, cmd, 0, 0, NLM_F_MULTI);
1346ae0a628SYotam Gigi if (!err)
1356ae0a628SYotam Gigi genlmsg_multicast_netns(&psample_nl_family, group->net, msg, 0,
1366ae0a628SYotam Gigi PSAMPLE_NL_MCGRP_CONFIG, GFP_ATOMIC);
1376ae0a628SYotam Gigi else
1386ae0a628SYotam Gigi nlmsg_free(msg);
1396ae0a628SYotam Gigi }
1406ae0a628SYotam Gigi
psample_group_create(struct net * net,u32 group_num)1416ae0a628SYotam Gigi static struct psample_group *psample_group_create(struct net *net,
1426ae0a628SYotam Gigi u32 group_num)
1436ae0a628SYotam Gigi {
1446ae0a628SYotam Gigi struct psample_group *group;
1456ae0a628SYotam Gigi
1466ae0a628SYotam Gigi group = kzalloc(sizeof(*group), GFP_ATOMIC);
1476ae0a628SYotam Gigi if (!group)
1486ae0a628SYotam Gigi return NULL;
1496ae0a628SYotam Gigi
1506ae0a628SYotam Gigi group->net = net;
1516ae0a628SYotam Gigi group->group_num = group_num;
1526ae0a628SYotam Gigi list_add_tail(&group->list, &psample_groups_list);
1536ae0a628SYotam Gigi
1546ae0a628SYotam Gigi psample_group_notify(group, PSAMPLE_CMD_NEW_GROUP);
1556ae0a628SYotam Gigi return group;
1566ae0a628SYotam Gigi }
1576ae0a628SYotam Gigi
psample_group_destroy(struct psample_group * group)1586ae0a628SYotam Gigi static void psample_group_destroy(struct psample_group *group)
1596ae0a628SYotam Gigi {
1606ae0a628SYotam Gigi psample_group_notify(group, PSAMPLE_CMD_DEL_GROUP);
1616ae0a628SYotam Gigi list_del(&group->list);
162dbf47a2aSVlad Buslov kfree_rcu(group, rcu);
1636ae0a628SYotam Gigi }
1646ae0a628SYotam Gigi
1656ae0a628SYotam Gigi static struct psample_group *
psample_group_lookup(struct net * net,u32 group_num)1666ae0a628SYotam Gigi psample_group_lookup(struct net *net, u32 group_num)
1676ae0a628SYotam Gigi {
1686ae0a628SYotam Gigi struct psample_group *group;
1696ae0a628SYotam Gigi
1706ae0a628SYotam Gigi list_for_each_entry(group, &psample_groups_list, list)
1716ae0a628SYotam Gigi if ((group->group_num == group_num) && (group->net == net))
1726ae0a628SYotam Gigi return group;
1736ae0a628SYotam Gigi return NULL;
1746ae0a628SYotam Gigi }
1756ae0a628SYotam Gigi
psample_group_get(struct net * net,u32 group_num)1766ae0a628SYotam Gigi struct psample_group *psample_group_get(struct net *net, u32 group_num)
1776ae0a628SYotam Gigi {
1786ae0a628SYotam Gigi struct psample_group *group;
1796ae0a628SYotam Gigi
1804a5da47dSVlad Buslov spin_lock_bh(&psample_groups_lock);
1816ae0a628SYotam Gigi
1826ae0a628SYotam Gigi group = psample_group_lookup(net, group_num);
1836ae0a628SYotam Gigi if (!group) {
1846ae0a628SYotam Gigi group = psample_group_create(net, group_num);
1856ae0a628SYotam Gigi if (!group)
1866ae0a628SYotam Gigi goto out;
1876ae0a628SYotam Gigi }
1886ae0a628SYotam Gigi group->refcount++;
1896ae0a628SYotam Gigi
1906ae0a628SYotam Gigi out:
1914a5da47dSVlad Buslov spin_unlock_bh(&psample_groups_lock);
1926ae0a628SYotam Gigi return group;
1936ae0a628SYotam Gigi }
1946ae0a628SYotam Gigi EXPORT_SYMBOL_GPL(psample_group_get);
1956ae0a628SYotam Gigi
psample_group_take(struct psample_group * group)1964a5da47dSVlad Buslov void psample_group_take(struct psample_group *group)
1974a5da47dSVlad Buslov {
1984a5da47dSVlad Buslov spin_lock_bh(&psample_groups_lock);
1994a5da47dSVlad Buslov group->refcount++;
2004a5da47dSVlad Buslov spin_unlock_bh(&psample_groups_lock);
2014a5da47dSVlad Buslov }
2024a5da47dSVlad Buslov EXPORT_SYMBOL_GPL(psample_group_take);
2034a5da47dSVlad Buslov
psample_group_put(struct psample_group * group)2046ae0a628SYotam Gigi void psample_group_put(struct psample_group *group)
2056ae0a628SYotam Gigi {
2064a5da47dSVlad Buslov spin_lock_bh(&psample_groups_lock);
2076ae0a628SYotam Gigi
2086ae0a628SYotam Gigi if (--group->refcount == 0)
2096ae0a628SYotam Gigi psample_group_destroy(group);
2106ae0a628SYotam Gigi
2114a5da47dSVlad Buslov spin_unlock_bh(&psample_groups_lock);
2126ae0a628SYotam Gigi }
2136ae0a628SYotam Gigi EXPORT_SYMBOL_GPL(psample_group_put);
2146ae0a628SYotam Gigi
21507a7f308SRandy Dunlap #ifdef CONFIG_INET
__psample_ip_tun_to_nlattr(struct sk_buff * skb,struct ip_tunnel_info * tun_info)216d8bed686SChris Mi static int __psample_ip_tun_to_nlattr(struct sk_buff *skb,
217d8bed686SChris Mi struct ip_tunnel_info *tun_info)
218d8bed686SChris Mi {
219d8bed686SChris Mi unsigned short tun_proto = ip_tunnel_info_af(tun_info);
220d8bed686SChris Mi const void *tun_opts = ip_tunnel_info_opts(tun_info);
221d8bed686SChris Mi const struct ip_tunnel_key *tun_key = &tun_info->key;
222d8bed686SChris Mi int tun_opts_len = tun_info->options_len;
223d8bed686SChris Mi
224d8bed686SChris Mi if (tun_key->tun_flags & TUNNEL_KEY &&
225d8bed686SChris Mi nla_put_be64(skb, PSAMPLE_TUNNEL_KEY_ATTR_ID, tun_key->tun_id,
226d8bed686SChris Mi PSAMPLE_TUNNEL_KEY_ATTR_PAD))
227d8bed686SChris Mi return -EMSGSIZE;
228d8bed686SChris Mi
229d8bed686SChris Mi if (tun_info->mode & IP_TUNNEL_INFO_BRIDGE &&
230d8bed686SChris Mi nla_put_flag(skb, PSAMPLE_TUNNEL_KEY_ATTR_IPV4_INFO_BRIDGE))
231d8bed686SChris Mi return -EMSGSIZE;
232d8bed686SChris Mi
233d8bed686SChris Mi switch (tun_proto) {
234d8bed686SChris Mi case AF_INET:
235d8bed686SChris Mi if (tun_key->u.ipv4.src &&
236d8bed686SChris Mi nla_put_in_addr(skb, PSAMPLE_TUNNEL_KEY_ATTR_IPV4_SRC,
237d8bed686SChris Mi tun_key->u.ipv4.src))
238d8bed686SChris Mi return -EMSGSIZE;
239d8bed686SChris Mi if (tun_key->u.ipv4.dst &&
240d8bed686SChris Mi nla_put_in_addr(skb, PSAMPLE_TUNNEL_KEY_ATTR_IPV4_DST,
241d8bed686SChris Mi tun_key->u.ipv4.dst))
242d8bed686SChris Mi return -EMSGSIZE;
243d8bed686SChris Mi break;
244d8bed686SChris Mi case AF_INET6:
245d8bed686SChris Mi if (!ipv6_addr_any(&tun_key->u.ipv6.src) &&
246d8bed686SChris Mi nla_put_in6_addr(skb, PSAMPLE_TUNNEL_KEY_ATTR_IPV6_SRC,
247d8bed686SChris Mi &tun_key->u.ipv6.src))
248d8bed686SChris Mi return -EMSGSIZE;
249d8bed686SChris Mi if (!ipv6_addr_any(&tun_key->u.ipv6.dst) &&
250d8bed686SChris Mi nla_put_in6_addr(skb, PSAMPLE_TUNNEL_KEY_ATTR_IPV6_DST,
251d8bed686SChris Mi &tun_key->u.ipv6.dst))
252d8bed686SChris Mi return -EMSGSIZE;
253d8bed686SChris Mi break;
254d8bed686SChris Mi }
255d8bed686SChris Mi if (tun_key->tos &&
256d8bed686SChris Mi nla_put_u8(skb, PSAMPLE_TUNNEL_KEY_ATTR_TOS, tun_key->tos))
257d8bed686SChris Mi return -EMSGSIZE;
258d8bed686SChris Mi if (nla_put_u8(skb, PSAMPLE_TUNNEL_KEY_ATTR_TTL, tun_key->ttl))
259d8bed686SChris Mi return -EMSGSIZE;
260d8bed686SChris Mi if ((tun_key->tun_flags & TUNNEL_DONT_FRAGMENT) &&
261d8bed686SChris Mi nla_put_flag(skb, PSAMPLE_TUNNEL_KEY_ATTR_DONT_FRAGMENT))
262d8bed686SChris Mi return -EMSGSIZE;
263d8bed686SChris Mi if ((tun_key->tun_flags & TUNNEL_CSUM) &&
264d8bed686SChris Mi nla_put_flag(skb, PSAMPLE_TUNNEL_KEY_ATTR_CSUM))
265d8bed686SChris Mi return -EMSGSIZE;
266d8bed686SChris Mi if (tun_key->tp_src &&
267d8bed686SChris Mi nla_put_be16(skb, PSAMPLE_TUNNEL_KEY_ATTR_TP_SRC, tun_key->tp_src))
268d8bed686SChris Mi return -EMSGSIZE;
269d8bed686SChris Mi if (tun_key->tp_dst &&
270d8bed686SChris Mi nla_put_be16(skb, PSAMPLE_TUNNEL_KEY_ATTR_TP_DST, tun_key->tp_dst))
271d8bed686SChris Mi return -EMSGSIZE;
272d8bed686SChris Mi if ((tun_key->tun_flags & TUNNEL_OAM) &&
273d8bed686SChris Mi nla_put_flag(skb, PSAMPLE_TUNNEL_KEY_ATTR_OAM))
274d8bed686SChris Mi return -EMSGSIZE;
275d8bed686SChris Mi if (tun_opts_len) {
276d8bed686SChris Mi if (tun_key->tun_flags & TUNNEL_GENEVE_OPT &&
277d8bed686SChris Mi nla_put(skb, PSAMPLE_TUNNEL_KEY_ATTR_GENEVE_OPTS,
278d8bed686SChris Mi tun_opts_len, tun_opts))
279d8bed686SChris Mi return -EMSGSIZE;
280d8bed686SChris Mi else if (tun_key->tun_flags & TUNNEL_ERSPAN_OPT &&
281d8bed686SChris Mi nla_put(skb, PSAMPLE_TUNNEL_KEY_ATTR_ERSPAN_OPTS,
282d8bed686SChris Mi tun_opts_len, tun_opts))
283d8bed686SChris Mi return -EMSGSIZE;
284d8bed686SChris Mi }
285d8bed686SChris Mi
286d8bed686SChris Mi return 0;
287d8bed686SChris Mi }
288d8bed686SChris Mi
psample_ip_tun_to_nlattr(struct sk_buff * skb,struct ip_tunnel_info * tun_info)289d8bed686SChris Mi static int psample_ip_tun_to_nlattr(struct sk_buff *skb,
290d8bed686SChris Mi struct ip_tunnel_info *tun_info)
291d8bed686SChris Mi {
292d8bed686SChris Mi struct nlattr *nla;
293d8bed686SChris Mi int err;
294d8bed686SChris Mi
295d8bed686SChris Mi nla = nla_nest_start_noflag(skb, PSAMPLE_ATTR_TUNNEL);
296d8bed686SChris Mi if (!nla)
297d8bed686SChris Mi return -EMSGSIZE;
298d8bed686SChris Mi
299d8bed686SChris Mi err = __psample_ip_tun_to_nlattr(skb, tun_info);
300d8bed686SChris Mi if (err) {
301d8bed686SChris Mi nla_nest_cancel(skb, nla);
302d8bed686SChris Mi return err;
303d8bed686SChris Mi }
304d8bed686SChris Mi
305d8bed686SChris Mi nla_nest_end(skb, nla);
306d8bed686SChris Mi
307d8bed686SChris Mi return 0;
308d8bed686SChris Mi }
309d8bed686SChris Mi
psample_tunnel_meta_len(struct ip_tunnel_info * tun_info)310d8bed686SChris Mi static int psample_tunnel_meta_len(struct ip_tunnel_info *tun_info)
311d8bed686SChris Mi {
312d8bed686SChris Mi unsigned short tun_proto = ip_tunnel_info_af(tun_info);
313d8bed686SChris Mi const struct ip_tunnel_key *tun_key = &tun_info->key;
314d8bed686SChris Mi int tun_opts_len = tun_info->options_len;
315a93dcaadSChris Mi int sum = nla_total_size(0); /* PSAMPLE_ATTR_TUNNEL */
316d8bed686SChris Mi
317d8bed686SChris Mi if (tun_key->tun_flags & TUNNEL_KEY)
318a93dcaadSChris Mi sum += nla_total_size_64bit(sizeof(u64));
319d8bed686SChris Mi
320d8bed686SChris Mi if (tun_info->mode & IP_TUNNEL_INFO_BRIDGE)
321d8bed686SChris Mi sum += nla_total_size(0);
322d8bed686SChris Mi
323d8bed686SChris Mi switch (tun_proto) {
324d8bed686SChris Mi case AF_INET:
325d8bed686SChris Mi if (tun_key->u.ipv4.src)
326d8bed686SChris Mi sum += nla_total_size(sizeof(u32));
327d8bed686SChris Mi if (tun_key->u.ipv4.dst)
328d8bed686SChris Mi sum += nla_total_size(sizeof(u32));
329d8bed686SChris Mi break;
330d8bed686SChris Mi case AF_INET6:
331d8bed686SChris Mi if (!ipv6_addr_any(&tun_key->u.ipv6.src))
332d8bed686SChris Mi sum += nla_total_size(sizeof(struct in6_addr));
333d8bed686SChris Mi if (!ipv6_addr_any(&tun_key->u.ipv6.dst))
334d8bed686SChris Mi sum += nla_total_size(sizeof(struct in6_addr));
335d8bed686SChris Mi break;
336d8bed686SChris Mi }
337d8bed686SChris Mi if (tun_key->tos)
338d8bed686SChris Mi sum += nla_total_size(sizeof(u8));
339d8bed686SChris Mi sum += nla_total_size(sizeof(u8)); /* TTL */
340d8bed686SChris Mi if (tun_key->tun_flags & TUNNEL_DONT_FRAGMENT)
341d8bed686SChris Mi sum += nla_total_size(0);
342d8bed686SChris Mi if (tun_key->tun_flags & TUNNEL_CSUM)
343d8bed686SChris Mi sum += nla_total_size(0);
344d8bed686SChris Mi if (tun_key->tp_src)
345d8bed686SChris Mi sum += nla_total_size(sizeof(u16));
346d8bed686SChris Mi if (tun_key->tp_dst)
347d8bed686SChris Mi sum += nla_total_size(sizeof(u16));
348d8bed686SChris Mi if (tun_key->tun_flags & TUNNEL_OAM)
349d8bed686SChris Mi sum += nla_total_size(0);
350d8bed686SChris Mi if (tun_opts_len) {
351d8bed686SChris Mi if (tun_key->tun_flags & TUNNEL_GENEVE_OPT)
352d8bed686SChris Mi sum += nla_total_size(tun_opts_len);
353d8bed686SChris Mi else if (tun_key->tun_flags & TUNNEL_ERSPAN_OPT)
354d8bed686SChris Mi sum += nla_total_size(tun_opts_len);
355d8bed686SChris Mi }
356d8bed686SChris Mi
357d8bed686SChris Mi return sum;
358d8bed686SChris Mi }
35907a7f308SRandy Dunlap #endif
360d8bed686SChris Mi
psample_sample_packet(struct psample_group * group,struct sk_buff * skb,u32 sample_rate,const struct psample_metadata * md)3616ae0a628SYotam Gigi void psample_sample_packet(struct psample_group *group, struct sk_buff *skb,
362a03e99d3SIdo Schimmel u32 sample_rate, const struct psample_metadata *md)
3636ae0a628SYotam Gigi {
36407e1a580SIdo Schimmel ktime_t tstamp = ktime_get_real();
365a03e99d3SIdo Schimmel int out_ifindex = md->out_ifindex;
366a03e99d3SIdo Schimmel int in_ifindex = md->in_ifindex;
367a03e99d3SIdo Schimmel u32 trunc_size = md->trunc_size;
36807a7f308SRandy Dunlap #ifdef CONFIG_INET
369d8bed686SChris Mi struct ip_tunnel_info *tun_info;
37007a7f308SRandy Dunlap #endif
3716ae0a628SYotam Gigi struct sk_buff *nl_skb;
3726ae0a628SYotam Gigi int data_len;
3736ae0a628SYotam Gigi int meta_len;
3746ae0a628SYotam Gigi void *data;
3756ae0a628SYotam Gigi int ret;
3766ae0a628SYotam Gigi
3776ae0a628SYotam Gigi meta_len = (in_ifindex ? nla_total_size(sizeof(u16)) : 0) +
3786ae0a628SYotam Gigi (out_ifindex ? nla_total_size(sizeof(u16)) : 0) +
37907e1a580SIdo Schimmel (md->out_tc_valid ? nla_total_size(sizeof(u16)) : 0) +
38007e1a580SIdo Schimmel (md->out_tc_occ_valid ? nla_total_size_64bit(sizeof(u64)) : 0) +
38107e1a580SIdo Schimmel (md->latency_valid ? nla_total_size_64bit(sizeof(u64)) : 0) +
3826ae0a628SYotam Gigi nla_total_size(sizeof(u32)) + /* sample_rate */
3836ae0a628SYotam Gigi nla_total_size(sizeof(u32)) + /* orig_size */
3846ae0a628SYotam Gigi nla_total_size(sizeof(u32)) + /* group_num */
38507e1a580SIdo Schimmel nla_total_size(sizeof(u32)) + /* seq */
38607e1a580SIdo Schimmel nla_total_size_64bit(sizeof(u64)) + /* timestamp */
38707e1a580SIdo Schimmel nla_total_size(sizeof(u16)); /* protocol */
3886ae0a628SYotam Gigi
38907a7f308SRandy Dunlap #ifdef CONFIG_INET
390d8bed686SChris Mi tun_info = skb_tunnel_info(skb);
391d8bed686SChris Mi if (tun_info)
392d8bed686SChris Mi meta_len += psample_tunnel_meta_len(tun_info);
39307a7f308SRandy Dunlap #endif
394d8bed686SChris Mi
3956ae0a628SYotam Gigi data_len = min(skb->len, trunc_size);
3966ae0a628SYotam Gigi if (meta_len + nla_total_size(data_len) > PSAMPLE_MAX_PACKET_SIZE)
3976ae0a628SYotam Gigi data_len = PSAMPLE_MAX_PACKET_SIZE - meta_len - NLA_HDRLEN
3986ae0a628SYotam Gigi - NLA_ALIGNTO;
3996ae0a628SYotam Gigi
4007eb9d767SNikolay Aleksandrov nl_skb = genlmsg_new(meta_len + nla_total_size(data_len), GFP_ATOMIC);
4016ae0a628SYotam Gigi if (unlikely(!nl_skb))
4026ae0a628SYotam Gigi return;
4036ae0a628SYotam Gigi
4046ae0a628SYotam Gigi data = genlmsg_put(nl_skb, 0, 0, &psample_nl_family, 0,
4056ae0a628SYotam Gigi PSAMPLE_CMD_SAMPLE);
4066ae0a628SYotam Gigi if (unlikely(!data))
4076ae0a628SYotam Gigi goto error;
4086ae0a628SYotam Gigi
4096ae0a628SYotam Gigi if (in_ifindex) {
4106ae0a628SYotam Gigi ret = nla_put_u16(nl_skb, PSAMPLE_ATTR_IIFINDEX, in_ifindex);
4116ae0a628SYotam Gigi if (unlikely(ret < 0))
4126ae0a628SYotam Gigi goto error;
4136ae0a628SYotam Gigi }
4146ae0a628SYotam Gigi
4156ae0a628SYotam Gigi if (out_ifindex) {
4166ae0a628SYotam Gigi ret = nla_put_u16(nl_skb, PSAMPLE_ATTR_OIFINDEX, out_ifindex);
4176ae0a628SYotam Gigi if (unlikely(ret < 0))
4186ae0a628SYotam Gigi goto error;
4196ae0a628SYotam Gigi }
4206ae0a628SYotam Gigi
4216ae0a628SYotam Gigi ret = nla_put_u32(nl_skb, PSAMPLE_ATTR_SAMPLE_RATE, sample_rate);
4226ae0a628SYotam Gigi if (unlikely(ret < 0))
4236ae0a628SYotam Gigi goto error;
4246ae0a628SYotam Gigi
4256ae0a628SYotam Gigi ret = nla_put_u32(nl_skb, PSAMPLE_ATTR_ORIGSIZE, skb->len);
4266ae0a628SYotam Gigi if (unlikely(ret < 0))
4276ae0a628SYotam Gigi goto error;
4286ae0a628SYotam Gigi
4296ae0a628SYotam Gigi ret = nla_put_u32(nl_skb, PSAMPLE_ATTR_SAMPLE_GROUP, group->group_num);
4306ae0a628SYotam Gigi if (unlikely(ret < 0))
4316ae0a628SYotam Gigi goto error;
4326ae0a628SYotam Gigi
4336ae0a628SYotam Gigi ret = nla_put_u32(nl_skb, PSAMPLE_ATTR_GROUP_SEQ, group->seq++);
4346ae0a628SYotam Gigi if (unlikely(ret < 0))
4356ae0a628SYotam Gigi goto error;
4366ae0a628SYotam Gigi
43707e1a580SIdo Schimmel if (md->out_tc_valid) {
43807e1a580SIdo Schimmel ret = nla_put_u16(nl_skb, PSAMPLE_ATTR_OUT_TC, md->out_tc);
43907e1a580SIdo Schimmel if (unlikely(ret < 0))
44007e1a580SIdo Schimmel goto error;
44107e1a580SIdo Schimmel }
44207e1a580SIdo Schimmel
44307e1a580SIdo Schimmel if (md->out_tc_occ_valid) {
44407e1a580SIdo Schimmel ret = nla_put_u64_64bit(nl_skb, PSAMPLE_ATTR_OUT_TC_OCC,
44507e1a580SIdo Schimmel md->out_tc_occ, PSAMPLE_ATTR_PAD);
44607e1a580SIdo Schimmel if (unlikely(ret < 0))
44707e1a580SIdo Schimmel goto error;
44807e1a580SIdo Schimmel }
44907e1a580SIdo Schimmel
45007e1a580SIdo Schimmel if (md->latency_valid) {
45107e1a580SIdo Schimmel ret = nla_put_u64_64bit(nl_skb, PSAMPLE_ATTR_LATENCY,
45207e1a580SIdo Schimmel md->latency, PSAMPLE_ATTR_PAD);
45307e1a580SIdo Schimmel if (unlikely(ret < 0))
45407e1a580SIdo Schimmel goto error;
45507e1a580SIdo Schimmel }
45607e1a580SIdo Schimmel
45707e1a580SIdo Schimmel ret = nla_put_u64_64bit(nl_skb, PSAMPLE_ATTR_TIMESTAMP,
45807e1a580SIdo Schimmel ktime_to_ns(tstamp), PSAMPLE_ATTR_PAD);
45907e1a580SIdo Schimmel if (unlikely(ret < 0))
46007e1a580SIdo Schimmel goto error;
46107e1a580SIdo Schimmel
46207e1a580SIdo Schimmel ret = nla_put_u16(nl_skb, PSAMPLE_ATTR_PROTO,
46307e1a580SIdo Schimmel be16_to_cpu(skb->protocol));
46407e1a580SIdo Schimmel if (unlikely(ret < 0))
46507e1a580SIdo Schimmel goto error;
46607e1a580SIdo Schimmel
4676ae0a628SYotam Gigi if (data_len) {
4686ae0a628SYotam Gigi int nla_len = nla_total_size(data_len);
4696ae0a628SYotam Gigi struct nlattr *nla;
4706ae0a628SYotam Gigi
4714df864c1SJohannes Berg nla = skb_put(nl_skb, nla_len);
4726ae0a628SYotam Gigi nla->nla_type = PSAMPLE_ATTR_DATA;
4736ae0a628SYotam Gigi nla->nla_len = nla_attr_size(data_len);
4746ae0a628SYotam Gigi
4756ae0a628SYotam Gigi if (skb_copy_bits(skb, 0, nla_data(nla), data_len))
4766ae0a628SYotam Gigi goto error;
4776ae0a628SYotam Gigi }
4786ae0a628SYotam Gigi
47907a7f308SRandy Dunlap #ifdef CONFIG_INET
480d8bed686SChris Mi if (tun_info) {
481d8bed686SChris Mi ret = psample_ip_tun_to_nlattr(nl_skb, tun_info);
482d8bed686SChris Mi if (unlikely(ret < 0))
483d8bed686SChris Mi goto error;
484d8bed686SChris Mi }
48507a7f308SRandy Dunlap #endif
486d8bed686SChris Mi
4876ae0a628SYotam Gigi genlmsg_end(nl_skb, data);
4886ae0a628SYotam Gigi genlmsg_multicast_netns(&psample_nl_family, group->net, nl_skb, 0,
4896ae0a628SYotam Gigi PSAMPLE_NL_MCGRP_SAMPLE, GFP_ATOMIC);
4906ae0a628SYotam Gigi
4916ae0a628SYotam Gigi return;
4926ae0a628SYotam Gigi error:
4936ae0a628SYotam Gigi pr_err_ratelimited("Could not create psample log message\n");
4946ae0a628SYotam Gigi nlmsg_free(nl_skb);
4956ae0a628SYotam Gigi }
4966ae0a628SYotam Gigi EXPORT_SYMBOL_GPL(psample_sample_packet);
4976ae0a628SYotam Gigi
psample_module_init(void)4986ae0a628SYotam Gigi static int __init psample_module_init(void)
4996ae0a628SYotam Gigi {
5006ae0a628SYotam Gigi return genl_register_family(&psample_nl_family);
5016ae0a628SYotam Gigi }
5026ae0a628SYotam Gigi
psample_module_exit(void)5036ae0a628SYotam Gigi static void __exit psample_module_exit(void)
5046ae0a628SYotam Gigi {
5056ae0a628SYotam Gigi genl_unregister_family(&psample_nl_family);
5066ae0a628SYotam Gigi }
5076ae0a628SYotam Gigi
5086ae0a628SYotam Gigi module_init(psample_module_init);
5096ae0a628SYotam Gigi module_exit(psample_module_exit);
5106ae0a628SYotam Gigi
511f1fd20c3SYotam Gigi MODULE_AUTHOR("Yotam Gigi <yotam.gi@gmail.com>");
5126ae0a628SYotam Gigi MODULE_DESCRIPTION("netlink channel for packet sampling");
5136ae0a628SYotam Gigi MODULE_LICENSE("GPL v2");
514