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 | |
23 | struct 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 | |
38 | static 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 | |
81 | static 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 | |
111 | static 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 | |
128 | out: |
129 | delay = msecs_to_jiffies(NSIM_PSAMPLE_REPORT_INTERVAL_MS); |
130 | schedule_delayed_work(dwork: &psample->psample_dw, delay); |
131 | } |
132 | |
133 | static 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 | |
156 | static 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 | |
171 | static 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 | |
191 | static 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 | |
198 | int 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 | |
252 | err_psample_free: |
253 | kfree(objp: nsim_dev->psample); |
254 | return err; |
255 | } |
256 | |
257 | void 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 | |