266 lines · c
1// SPDX-License-Identifier: GPL-2.02/* 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 10020#define NSIM_PSAMPLE_INVALID_TC 0xFFFF21#define NSIM_PSAMPLE_L4_DATA_LEN 10022 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, 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 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(&out_tc_occ, 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(&latency, 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, skb->len);125 psample_sample_packet(psample->group, skb, psample->rate, &md);126 consume_skb(skb);127 128out:129 delay = msecs_to_jiffies(NSIM_PSAMPLE_REPORT_INTERVAL_MS);130 schedule_delayed_work(&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(nsim_dev);143 psample->group = psample_group_get(devlink_net(devlink),144 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(&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(&psample->psample_dw);166 psample_group_put(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(data, count, &enable);180 if (err)181 return err;182 183 if (enable)184 err = nsim_dev_psample_enable(nsim_dev);185 else186 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(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("psample", nsim_dev->ddir);211 if (IS_ERR(psample->ddir)) {212 err = PTR_ERR(psample->ddir);213 goto err_psample_free;214 }215 216 /* Populate sampling parameters with sane defaults. */217 psample->rate = 100;218 debugfs_create_u32("rate", 0600, psample->ddir, &psample->rate);219 220 psample->group_num = 10;221 debugfs_create_u32("group_num", 0600, psample->ddir,222 &psample->group_num);223 224 psample->trunc_size = 0;225 debugfs_create_u32("trunc_size", 0600, psample->ddir,226 &psample->trunc_size);227 228 psample->in_ifindex = 1;229 debugfs_create_u32("in_ifindex", 0600, psample->ddir,230 &psample->in_ifindex);231 232 psample->out_ifindex = 2;233 debugfs_create_u32("out_ifindex", 0600, psample->ddir,234 &psample->out_ifindex);235 236 psample->out_tc = 0;237 debugfs_create_u16("out_tc", 0600, psample->ddir, &psample->out_tc);238 239 psample->out_tc_occ_max = 10000;240 debugfs_create_u64("out_tc_occ_max", 0600, psample->ddir,241 &psample->out_tc_occ_max);242 243 psample->latency_max = 50;244 debugfs_create_u64("latency_max", 0600, psample->ddir,245 &psample->latency_max);246 247 debugfs_create_file("enable", 0200, psample->ddir, nsim_dev,248 &nsim_psample_enable_fops);249 250 return 0;251 252err_psample_free:253 kfree(nsim_dev->psample);254 return err;255}256 257void nsim_dev_psample_exit(struct nsim_dev *nsim_dev)258{259 debugfs_remove_recursive(nsim_dev->psample->ddir);260 if (nsim_dev->psample->is_active) {261 cancel_delayed_work_sync(&nsim_dev->psample->psample_dw);262 psample_group_put(nsim_dev->psample->group);263 }264 kfree(nsim_dev->psample);265}266