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

source code of linux/drivers/net/netdevsim/psample.c