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, sizeof(struct ethhdr)); 53 eth_random_addr(eth->h_dest); 54 eth_random_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, skb->len); 59 iph = skb_put(skb, sizeof(struct iphdr)); 60 iph->protocol = IPPROTO_UDP; 61 iph->saddr = in_aton("192.0.2.1"); 62 iph->daddr = in_aton("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((unsigned char *)iph, iph->ihl); 71 72 skb_set_transport_header(skb, skb->len); 73 udph = skb_put_zero(skb, sizeof(struct udphdr) + data_len); 74 get_random_bytes(&udph->source, sizeof(u16)); 75 get_random_bytes(&udph->dest, 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 { 84 md->trunc_size = psample->trunc_size; 85 md->in_ifindex = psample->in_ifindex; 86 md->out_ifindex = psample->out_ifindex; 87 88 if (psample->out_tc != NSIM_PSAMPLE_INVALID_TC) { 89 md->out_tc = psample->out_tc; 90 md->out_tc_valid = 1; 91 } 92 93 if (psample->out_tc_occ_max) { 94 u64 out_tc_occ; 95 96 get_random_bytes(&out_tc_occ, sizeof(u64)); 97 md->out_tc_occ = out_tc_occ & (psample->out_tc_occ_max - 1); 98 md->out_tc_occ_valid = 1; 99 } 100 101 if (psample->latency_max) { 102 u64 latency; 103 104 get_random_bytes(&latency, sizeof(u64)); 105 md->latency = latency & (psample->latency_max - 1); 106 md->latency_valid = 1; 107 } 108 } 109 110 static void nsim_dev_psample_report_work(struct work_struct *work) 111 { 112 struct nsim_dev_psample *psample; 113 struct psample_metadata md = {}; 114 struct sk_buff *skb; 115 unsigned long delay; 116 117 psample = container_of(work, struct nsim_dev_psample, psample_dw.work); 118 119 skb = nsim_dev_psample_skb_build(); 120 if (!skb) 121 goto out; 122 123 nsim_dev_psample_md_prepare(psample, &md); 124 psample_sample_packet(psample->group, skb, psample->rate, &md); 125 consume_skb(skb); 126 127 out: 128 delay = msecs_to_jiffies(NSIM_PSAMPLE_REPORT_INTERVAL_MS); 129 schedule_delayed_work(&psample->psample_dw, delay); 130 } 131 132 static int nsim_dev_psample_enable(struct nsim_dev *nsim_dev) 133 { 134 struct nsim_dev_psample *psample = nsim_dev->psample; 135 struct devlink *devlink; 136 unsigned long delay; 137 138 if (psample->is_active) 139 return -EBUSY; 140 141 devlink = priv_to_devlink(nsim_dev); 142 psample->group = psample_group_get(devlink_net(devlink), 143 psample->group_num); 144 if (!psample->group) 145 return -EINVAL; 146 147 delay = msecs_to_jiffies(NSIM_PSAMPLE_REPORT_INTERVAL_MS); 148 schedule_delayed_work(&psample->psample_dw, delay); 149 150 psample->is_active = true; 151 152 return 0; 153 } 154 155 static int nsim_dev_psample_disable(struct nsim_dev *nsim_dev) 156 { 157 struct nsim_dev_psample *psample = nsim_dev->psample; 158 159 if (!psample->is_active) 160 return -EINVAL; 161 162 psample->is_active = false; 163 164 cancel_delayed_work_sync(&psample->psample_dw); 165 psample_group_put(psample->group); 166 167 return 0; 168 } 169 170 static ssize_t nsim_dev_psample_enable_write(struct file *file, 171 const char __user *data, 172 size_t count, loff_t *ppos) 173 { 174 struct nsim_dev *nsim_dev = file->private_data; 175 bool enable; 176 int err; 177 178 err = kstrtobool_from_user(data, count, &enable); 179 if (err) 180 return err; 181 182 if (enable) 183 err = nsim_dev_psample_enable(nsim_dev); 184 else 185 err = nsim_dev_psample_disable(nsim_dev); 186 187 return err ? err : count; 188 } 189 190 static const struct file_operations nsim_psample_enable_fops = { 191 .open = simple_open, 192 .write = nsim_dev_psample_enable_write, 193 .llseek = generic_file_llseek, 194 .owner = THIS_MODULE, 195 }; 196 197 int nsim_dev_psample_init(struct nsim_dev *nsim_dev) 198 { 199 struct nsim_dev_psample *psample; 200 int err; 201 202 psample = kzalloc(sizeof(*psample), GFP_KERNEL); 203 if (!psample) 204 return -ENOMEM; 205 nsim_dev->psample = psample; 206 207 INIT_DELAYED_WORK(&psample->psample_dw, nsim_dev_psample_report_work); 208 209 psample->ddir = debugfs_create_dir("psample", nsim_dev->ddir); 210 if (IS_ERR(psample->ddir)) { 211 err = PTR_ERR(psample->ddir); 212 goto err_psample_free; 213 } 214 215 /* Populate sampling parameters with sane defaults. */ 216 psample->rate = 100; 217 debugfs_create_u32("rate", 0600, psample->ddir, &psample->rate); 218 219 psample->group_num = 10; 220 debugfs_create_u32("group_num", 0600, psample->ddir, 221 &psample->group_num); 222 223 psample->trunc_size = 0; 224 debugfs_create_u32("trunc_size", 0600, psample->ddir, 225 &psample->trunc_size); 226 227 psample->in_ifindex = 1; 228 debugfs_create_u32("in_ifindex", 0600, psample->ddir, 229 &psample->in_ifindex); 230 231 psample->out_ifindex = 2; 232 debugfs_create_u32("out_ifindex", 0600, psample->ddir, 233 &psample->out_ifindex); 234 235 psample->out_tc = 0; 236 debugfs_create_u16("out_tc", 0600, psample->ddir, &psample->out_tc); 237 238 psample->out_tc_occ_max = 10000; 239 debugfs_create_u64("out_tc_occ_max", 0600, psample->ddir, 240 &psample->out_tc_occ_max); 241 242 psample->latency_max = 50; 243 debugfs_create_u64("latency_max", 0600, psample->ddir, 244 &psample->latency_max); 245 246 debugfs_create_file("enable", 0200, psample->ddir, nsim_dev, 247 &nsim_psample_enable_fops); 248 249 return 0; 250 251 err_psample_free: 252 kfree(nsim_dev->psample); 253 return err; 254 } 255 256 void nsim_dev_psample_exit(struct nsim_dev *nsim_dev) 257 { 258 debugfs_remove_recursive(nsim_dev->psample->ddir); 259 if (nsim_dev->psample->is_active) { 260 cancel_delayed_work_sync(&nsim_dev->psample->psample_dw); 261 psample_group_put(nsim_dev->psample->group); 262 } 263 kfree(nsim_dev->psample); 264 } 265