1a8700c3dSIdo Schimmel // SPDX-License-Identifier: GPL-2.0
2a8700c3dSIdo Schimmel /* Copyright (c) 2021 Mellanox Technologies. All rights reserved */
3a8700c3dSIdo Schimmel
4a8700c3dSIdo Schimmel #include <linux/debugfs.h>
5a8700c3dSIdo Schimmel #include <linux/err.h>
6a8700c3dSIdo Schimmel #include <linux/etherdevice.h>
7a8700c3dSIdo Schimmel #include <linux/inet.h>
8a8700c3dSIdo Schimmel #include <linux/kernel.h>
9a8700c3dSIdo Schimmel #include <linux/random.h>
10a8700c3dSIdo Schimmel #include <linux/slab.h>
11a8700c3dSIdo Schimmel #include <net/devlink.h>
12a8700c3dSIdo Schimmel #include <net/ip.h>
13a8700c3dSIdo Schimmel #include <net/psample.h>
14a8700c3dSIdo Schimmel #include <uapi/linux/ip.h>
15a8700c3dSIdo Schimmel #include <uapi/linux/udp.h>
16a8700c3dSIdo Schimmel
17a8700c3dSIdo Schimmel #include "netdevsim.h"
18a8700c3dSIdo Schimmel
19a8700c3dSIdo Schimmel #define NSIM_PSAMPLE_REPORT_INTERVAL_MS 100
20a8700c3dSIdo Schimmel #define NSIM_PSAMPLE_INVALID_TC 0xFFFF
21a8700c3dSIdo Schimmel #define NSIM_PSAMPLE_L4_DATA_LEN 100
22a8700c3dSIdo Schimmel
23a8700c3dSIdo Schimmel struct nsim_dev_psample {
24a8700c3dSIdo Schimmel struct delayed_work psample_dw;
25a8700c3dSIdo Schimmel struct dentry *ddir;
26a8700c3dSIdo Schimmel struct psample_group *group;
27a8700c3dSIdo Schimmel u32 rate;
28a8700c3dSIdo Schimmel u32 group_num;
29a8700c3dSIdo Schimmel u32 trunc_size;
30a8700c3dSIdo Schimmel int in_ifindex;
31a8700c3dSIdo Schimmel int out_ifindex;
32a8700c3dSIdo Schimmel u16 out_tc;
33a8700c3dSIdo Schimmel u64 out_tc_occ_max;
34a8700c3dSIdo Schimmel u64 latency_max;
35a8700c3dSIdo Schimmel bool is_active;
36a8700c3dSIdo Schimmel };
37a8700c3dSIdo Schimmel
nsim_dev_psample_skb_build(void)38a8700c3dSIdo Schimmel static struct sk_buff *nsim_dev_psample_skb_build(void)
39a8700c3dSIdo Schimmel {
40a8700c3dSIdo Schimmel int tot_len, data_len = NSIM_PSAMPLE_L4_DATA_LEN;
41a8700c3dSIdo Schimmel struct sk_buff *skb;
42a8700c3dSIdo Schimmel struct udphdr *udph;
43a8700c3dSIdo Schimmel struct ethhdr *eth;
44a8700c3dSIdo Schimmel struct iphdr *iph;
45a8700c3dSIdo Schimmel
46a8700c3dSIdo Schimmel skb = alloc_skb(NLMSG_GOODSIZE, GFP_KERNEL);
47a8700c3dSIdo Schimmel if (!skb)
48a8700c3dSIdo Schimmel return NULL;
49a8700c3dSIdo Schimmel tot_len = sizeof(struct iphdr) + sizeof(struct udphdr) + data_len;
50a8700c3dSIdo Schimmel
51a8700c3dSIdo Schimmel skb_reset_mac_header(skb);
52a8700c3dSIdo Schimmel eth = skb_put(skb, sizeof(struct ethhdr));
53a8700c3dSIdo Schimmel eth_random_addr(eth->h_dest);
54a8700c3dSIdo Schimmel eth_random_addr(eth->h_source);
55a8700c3dSIdo Schimmel eth->h_proto = htons(ETH_P_IP);
56a8700c3dSIdo Schimmel skb->protocol = htons(ETH_P_IP);
57a8700c3dSIdo Schimmel
58a8700c3dSIdo Schimmel skb_set_network_header(skb, skb->len);
59a8700c3dSIdo Schimmel iph = skb_put(skb, sizeof(struct iphdr));
60a8700c3dSIdo Schimmel iph->protocol = IPPROTO_UDP;
61a8700c3dSIdo Schimmel iph->saddr = in_aton("192.0.2.1");
62a8700c3dSIdo Schimmel iph->daddr = in_aton("198.51.100.1");
63a8700c3dSIdo Schimmel iph->version = 0x4;
64a8700c3dSIdo Schimmel iph->frag_off = 0;
65a8700c3dSIdo Schimmel iph->ihl = 0x5;
66a8700c3dSIdo Schimmel iph->tot_len = htons(tot_len);
67a8700c3dSIdo Schimmel iph->id = 0;
68a8700c3dSIdo Schimmel iph->ttl = 100;
69a8700c3dSIdo Schimmel iph->check = 0;
70a8700c3dSIdo Schimmel iph->check = ip_fast_csum((unsigned char *)iph, iph->ihl);
71a8700c3dSIdo Schimmel
72a8700c3dSIdo Schimmel skb_set_transport_header(skb, skb->len);
73a8700c3dSIdo Schimmel udph = skb_put_zero(skb, sizeof(struct udphdr) + data_len);
74a8700c3dSIdo Schimmel get_random_bytes(&udph->source, sizeof(u16));
75a8700c3dSIdo Schimmel get_random_bytes(&udph->dest, sizeof(u16));
76a8700c3dSIdo Schimmel udph->len = htons(sizeof(struct udphdr) + data_len);
77a8700c3dSIdo Schimmel
78a8700c3dSIdo Schimmel return skb;
79a8700c3dSIdo Schimmel }
80a8700c3dSIdo Schimmel
nsim_dev_psample_md_prepare(const struct nsim_dev_psample * psample,struct psample_metadata * md,unsigned int len)81a8700c3dSIdo Schimmel static void nsim_dev_psample_md_prepare(const struct nsim_dev_psample *psample,
82*a9b5d871SIdo Schimmel struct psample_metadata *md,
83*a9b5d871SIdo Schimmel unsigned int len)
84a8700c3dSIdo Schimmel {
85*a9b5d871SIdo Schimmel md->trunc_size = psample->trunc_size ? psample->trunc_size : len;
86a8700c3dSIdo Schimmel md->in_ifindex = psample->in_ifindex;
87a8700c3dSIdo Schimmel md->out_ifindex = psample->out_ifindex;
88a8700c3dSIdo Schimmel
89a8700c3dSIdo Schimmel if (psample->out_tc != NSIM_PSAMPLE_INVALID_TC) {
90a8700c3dSIdo Schimmel md->out_tc = psample->out_tc;
91a8700c3dSIdo Schimmel md->out_tc_valid = 1;
92a8700c3dSIdo Schimmel }
93a8700c3dSIdo Schimmel
94a8700c3dSIdo Schimmel if (psample->out_tc_occ_max) {
95a8700c3dSIdo Schimmel u64 out_tc_occ;
96a8700c3dSIdo Schimmel
97a8700c3dSIdo Schimmel get_random_bytes(&out_tc_occ, sizeof(u64));
98a8700c3dSIdo Schimmel md->out_tc_occ = out_tc_occ & (psample->out_tc_occ_max - 1);
99a8700c3dSIdo Schimmel md->out_tc_occ_valid = 1;
100a8700c3dSIdo Schimmel }
101a8700c3dSIdo Schimmel
102a8700c3dSIdo Schimmel if (psample->latency_max) {
103a8700c3dSIdo Schimmel u64 latency;
104a8700c3dSIdo Schimmel
105a8700c3dSIdo Schimmel get_random_bytes(&latency, sizeof(u64));
106a8700c3dSIdo Schimmel md->latency = latency & (psample->latency_max - 1);
107a8700c3dSIdo Schimmel md->latency_valid = 1;
108a8700c3dSIdo Schimmel }
109a8700c3dSIdo Schimmel }
110a8700c3dSIdo Schimmel
nsim_dev_psample_report_work(struct work_struct * work)111a8700c3dSIdo Schimmel static void nsim_dev_psample_report_work(struct work_struct *work)
112a8700c3dSIdo Schimmel {
113a8700c3dSIdo Schimmel struct nsim_dev_psample *psample;
114a8700c3dSIdo Schimmel struct psample_metadata md = {};
115a8700c3dSIdo Schimmel struct sk_buff *skb;
116a8700c3dSIdo Schimmel unsigned long delay;
117a8700c3dSIdo Schimmel
118a8700c3dSIdo Schimmel psample = container_of(work, struct nsim_dev_psample, psample_dw.work);
119a8700c3dSIdo Schimmel
120a8700c3dSIdo Schimmel skb = nsim_dev_psample_skb_build();
121a8700c3dSIdo Schimmel if (!skb)
122a8700c3dSIdo Schimmel goto out;
123a8700c3dSIdo Schimmel
124*a9b5d871SIdo Schimmel nsim_dev_psample_md_prepare(psample, &md, skb->len);
125a8700c3dSIdo Schimmel psample_sample_packet(psample->group, skb, psample->rate, &md);
126a8700c3dSIdo Schimmel consume_skb(skb);
127a8700c3dSIdo Schimmel
128a8700c3dSIdo Schimmel out:
129a8700c3dSIdo Schimmel delay = msecs_to_jiffies(NSIM_PSAMPLE_REPORT_INTERVAL_MS);
130a8700c3dSIdo Schimmel schedule_delayed_work(&psample->psample_dw, delay);
131a8700c3dSIdo Schimmel }
132a8700c3dSIdo Schimmel
nsim_dev_psample_enable(struct nsim_dev * nsim_dev)133a8700c3dSIdo Schimmel static int nsim_dev_psample_enable(struct nsim_dev *nsim_dev)
134a8700c3dSIdo Schimmel {
135a8700c3dSIdo Schimmel struct nsim_dev_psample *psample = nsim_dev->psample;
136a8700c3dSIdo Schimmel struct devlink *devlink;
137a8700c3dSIdo Schimmel unsigned long delay;
138a8700c3dSIdo Schimmel
139a8700c3dSIdo Schimmel if (psample->is_active)
140a8700c3dSIdo Schimmel return -EBUSY;
141a8700c3dSIdo Schimmel
142a8700c3dSIdo Schimmel devlink = priv_to_devlink(nsim_dev);
143a8700c3dSIdo Schimmel psample->group = psample_group_get(devlink_net(devlink),
144a8700c3dSIdo Schimmel psample->group_num);
145a8700c3dSIdo Schimmel if (!psample->group)
146a8700c3dSIdo Schimmel return -EINVAL;
147a8700c3dSIdo Schimmel
148a8700c3dSIdo Schimmel delay = msecs_to_jiffies(NSIM_PSAMPLE_REPORT_INTERVAL_MS);
149a8700c3dSIdo Schimmel schedule_delayed_work(&psample->psample_dw, delay);
150a8700c3dSIdo Schimmel
151a8700c3dSIdo Schimmel psample->is_active = true;
152a8700c3dSIdo Schimmel
153a8700c3dSIdo Schimmel return 0;
154a8700c3dSIdo Schimmel }
155a8700c3dSIdo Schimmel
nsim_dev_psample_disable(struct nsim_dev * nsim_dev)156a8700c3dSIdo Schimmel static int nsim_dev_psample_disable(struct nsim_dev *nsim_dev)
157a8700c3dSIdo Schimmel {
158a8700c3dSIdo Schimmel struct nsim_dev_psample *psample = nsim_dev->psample;
159a8700c3dSIdo Schimmel
160a8700c3dSIdo Schimmel if (!psample->is_active)
161a8700c3dSIdo Schimmel return -EINVAL;
162a8700c3dSIdo Schimmel
163a8700c3dSIdo Schimmel psample->is_active = false;
164a8700c3dSIdo Schimmel
165a8700c3dSIdo Schimmel cancel_delayed_work_sync(&psample->psample_dw);
166a8700c3dSIdo Schimmel psample_group_put(psample->group);
167a8700c3dSIdo Schimmel
168a8700c3dSIdo Schimmel return 0;
169a8700c3dSIdo Schimmel }
170a8700c3dSIdo Schimmel
nsim_dev_psample_enable_write(struct file * file,const char __user * data,size_t count,loff_t * ppos)171a8700c3dSIdo Schimmel static ssize_t nsim_dev_psample_enable_write(struct file *file,
172a8700c3dSIdo Schimmel const char __user *data,
173a8700c3dSIdo Schimmel size_t count, loff_t *ppos)
174a8700c3dSIdo Schimmel {
175a8700c3dSIdo Schimmel struct nsim_dev *nsim_dev = file->private_data;
176a8700c3dSIdo Schimmel bool enable;
177a8700c3dSIdo Schimmel int err;
178a8700c3dSIdo Schimmel
179a8700c3dSIdo Schimmel err = kstrtobool_from_user(data, count, &enable);
180a8700c3dSIdo Schimmel if (err)
181a8700c3dSIdo Schimmel return err;
182a8700c3dSIdo Schimmel
183a8700c3dSIdo Schimmel if (enable)
184a8700c3dSIdo Schimmel err = nsim_dev_psample_enable(nsim_dev);
185a8700c3dSIdo Schimmel else
186a8700c3dSIdo Schimmel err = nsim_dev_psample_disable(nsim_dev);
187a8700c3dSIdo Schimmel
188a8700c3dSIdo Schimmel return err ? err : count;
189a8700c3dSIdo Schimmel }
190a8700c3dSIdo Schimmel
191a8700c3dSIdo Schimmel static const struct file_operations nsim_psample_enable_fops = {
192a8700c3dSIdo Schimmel .open = simple_open,
193a8700c3dSIdo Schimmel .write = nsim_dev_psample_enable_write,
194a8700c3dSIdo Schimmel .llseek = generic_file_llseek,
195a8700c3dSIdo Schimmel .owner = THIS_MODULE,
196a8700c3dSIdo Schimmel };
197a8700c3dSIdo Schimmel
nsim_dev_psample_init(struct nsim_dev * nsim_dev)198a8700c3dSIdo Schimmel int nsim_dev_psample_init(struct nsim_dev *nsim_dev)
199a8700c3dSIdo Schimmel {
200a8700c3dSIdo Schimmel struct nsim_dev_psample *psample;
201a8700c3dSIdo Schimmel int err;
202a8700c3dSIdo Schimmel
203a8700c3dSIdo Schimmel psample = kzalloc(sizeof(*psample), GFP_KERNEL);
204a8700c3dSIdo Schimmel if (!psample)
205a8700c3dSIdo Schimmel return -ENOMEM;
206a8700c3dSIdo Schimmel nsim_dev->psample = psample;
207a8700c3dSIdo Schimmel
208a8700c3dSIdo Schimmel INIT_DELAYED_WORK(&psample->psample_dw, nsim_dev_psample_report_work);
209a8700c3dSIdo Schimmel
210a8700c3dSIdo Schimmel psample->ddir = debugfs_create_dir("psample", nsim_dev->ddir);
211a8700c3dSIdo Schimmel if (IS_ERR(psample->ddir)) {
212a8700c3dSIdo Schimmel err = PTR_ERR(psample->ddir);
213a8700c3dSIdo Schimmel goto err_psample_free;
214a8700c3dSIdo Schimmel }
215a8700c3dSIdo Schimmel
216a8700c3dSIdo Schimmel /* Populate sampling parameters with sane defaults. */
217a8700c3dSIdo Schimmel psample->rate = 100;
218a8700c3dSIdo Schimmel debugfs_create_u32("rate", 0600, psample->ddir, &psample->rate);
219a8700c3dSIdo Schimmel
220a8700c3dSIdo Schimmel psample->group_num = 10;
221a8700c3dSIdo Schimmel debugfs_create_u32("group_num", 0600, psample->ddir,
222a8700c3dSIdo Schimmel &psample->group_num);
223a8700c3dSIdo Schimmel
224a8700c3dSIdo Schimmel psample->trunc_size = 0;
225a8700c3dSIdo Schimmel debugfs_create_u32("trunc_size", 0600, psample->ddir,
226a8700c3dSIdo Schimmel &psample->trunc_size);
227a8700c3dSIdo Schimmel
228a8700c3dSIdo Schimmel psample->in_ifindex = 1;
229a8700c3dSIdo Schimmel debugfs_create_u32("in_ifindex", 0600, psample->ddir,
230a8700c3dSIdo Schimmel &psample->in_ifindex);
231a8700c3dSIdo Schimmel
232a8700c3dSIdo Schimmel psample->out_ifindex = 2;
233a8700c3dSIdo Schimmel debugfs_create_u32("out_ifindex", 0600, psample->ddir,
234a8700c3dSIdo Schimmel &psample->out_ifindex);
235a8700c3dSIdo Schimmel
236a8700c3dSIdo Schimmel psample->out_tc = 0;
237a8700c3dSIdo Schimmel debugfs_create_u16("out_tc", 0600, psample->ddir, &psample->out_tc);
238a8700c3dSIdo Schimmel
239a8700c3dSIdo Schimmel psample->out_tc_occ_max = 10000;
240a8700c3dSIdo Schimmel debugfs_create_u64("out_tc_occ_max", 0600, psample->ddir,
241a8700c3dSIdo Schimmel &psample->out_tc_occ_max);
242a8700c3dSIdo Schimmel
243a8700c3dSIdo Schimmel psample->latency_max = 50;
244a8700c3dSIdo Schimmel debugfs_create_u64("latency_max", 0600, psample->ddir,
245a8700c3dSIdo Schimmel &psample->latency_max);
246a8700c3dSIdo Schimmel
247a8700c3dSIdo Schimmel debugfs_create_file("enable", 0200, psample->ddir, nsim_dev,
248a8700c3dSIdo Schimmel &nsim_psample_enable_fops);
249a8700c3dSIdo Schimmel
250a8700c3dSIdo Schimmel return 0;
251a8700c3dSIdo Schimmel
252a8700c3dSIdo Schimmel err_psample_free:
253a8700c3dSIdo Schimmel kfree(nsim_dev->psample);
254a8700c3dSIdo Schimmel return err;
255a8700c3dSIdo Schimmel }
256a8700c3dSIdo Schimmel
nsim_dev_psample_exit(struct nsim_dev * nsim_dev)257a8700c3dSIdo Schimmel void nsim_dev_psample_exit(struct nsim_dev *nsim_dev)
258a8700c3dSIdo Schimmel {
259a8700c3dSIdo Schimmel debugfs_remove_recursive(nsim_dev->psample->ddir);
260a8700c3dSIdo Schimmel if (nsim_dev->psample->is_active) {
261a8700c3dSIdo Schimmel cancel_delayed_work_sync(&nsim_dev->psample->psample_dw);
262a8700c3dSIdo Schimmel psample_group_put(nsim_dev->psample->group);
263a8700c3dSIdo Schimmel }
264a8700c3dSIdo Schimmel kfree(nsim_dev->psample);
265a8700c3dSIdo Schimmel }
266