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