0001
0002
0003
0004 #include <linux/debugfs.h>
0005 #include <linux/err.h>
0006 #include <linux/etherdevice.h>
0007 #include <linux/inet.h>
0008 #include <linux/kernel.h>
0009 #include <linux/random.h>
0010 #include <linux/slab.h>
0011 #include <net/devlink.h>
0012 #include <net/ip.h>
0013 #include <net/psample.h>
0014 #include <uapi/linux/ip.h>
0015 #include <uapi/linux/udp.h>
0016
0017 #include "netdevsim.h"
0018
0019 #define NSIM_PSAMPLE_REPORT_INTERVAL_MS 100
0020 #define NSIM_PSAMPLE_INVALID_TC 0xFFFF
0021 #define NSIM_PSAMPLE_L4_DATA_LEN 100
0022
0023 struct nsim_dev_psample {
0024 struct delayed_work psample_dw;
0025 struct dentry *ddir;
0026 struct psample_group *group;
0027 u32 rate;
0028 u32 group_num;
0029 u32 trunc_size;
0030 int in_ifindex;
0031 int out_ifindex;
0032 u16 out_tc;
0033 u64 out_tc_occ_max;
0034 u64 latency_max;
0035 bool is_active;
0036 };
0037
0038 static struct sk_buff *nsim_dev_psample_skb_build(void)
0039 {
0040 int tot_len, data_len = NSIM_PSAMPLE_L4_DATA_LEN;
0041 struct sk_buff *skb;
0042 struct udphdr *udph;
0043 struct ethhdr *eth;
0044 struct iphdr *iph;
0045
0046 skb = alloc_skb(NLMSG_GOODSIZE, GFP_KERNEL);
0047 if (!skb)
0048 return NULL;
0049 tot_len = sizeof(struct iphdr) + sizeof(struct udphdr) + data_len;
0050
0051 skb_reset_mac_header(skb);
0052 eth = skb_put(skb, sizeof(struct ethhdr));
0053 eth_random_addr(eth->h_dest);
0054 eth_random_addr(eth->h_source);
0055 eth->h_proto = htons(ETH_P_IP);
0056 skb->protocol = htons(ETH_P_IP);
0057
0058 skb_set_network_header(skb, skb->len);
0059 iph = skb_put(skb, sizeof(struct iphdr));
0060 iph->protocol = IPPROTO_UDP;
0061 iph->saddr = in_aton("192.0.2.1");
0062 iph->daddr = in_aton("198.51.100.1");
0063 iph->version = 0x4;
0064 iph->frag_off = 0;
0065 iph->ihl = 0x5;
0066 iph->tot_len = htons(tot_len);
0067 iph->id = 0;
0068 iph->ttl = 100;
0069 iph->check = 0;
0070 iph->check = ip_fast_csum((unsigned char *)iph, iph->ihl);
0071
0072 skb_set_transport_header(skb, skb->len);
0073 udph = skb_put_zero(skb, sizeof(struct udphdr) + data_len);
0074 get_random_bytes(&udph->source, sizeof(u16));
0075 get_random_bytes(&udph->dest, sizeof(u16));
0076 udph->len = htons(sizeof(struct udphdr) + data_len);
0077
0078 return skb;
0079 }
0080
0081 static void nsim_dev_psample_md_prepare(const struct nsim_dev_psample *psample,
0082 struct psample_metadata *md,
0083 unsigned int len)
0084 {
0085 md->trunc_size = psample->trunc_size ? psample->trunc_size : len;
0086 md->in_ifindex = psample->in_ifindex;
0087 md->out_ifindex = psample->out_ifindex;
0088
0089 if (psample->out_tc != NSIM_PSAMPLE_INVALID_TC) {
0090 md->out_tc = psample->out_tc;
0091 md->out_tc_valid = 1;
0092 }
0093
0094 if (psample->out_tc_occ_max) {
0095 u64 out_tc_occ;
0096
0097 get_random_bytes(&out_tc_occ, sizeof(u64));
0098 md->out_tc_occ = out_tc_occ & (psample->out_tc_occ_max - 1);
0099 md->out_tc_occ_valid = 1;
0100 }
0101
0102 if (psample->latency_max) {
0103 u64 latency;
0104
0105 get_random_bytes(&latency, sizeof(u64));
0106 md->latency = latency & (psample->latency_max - 1);
0107 md->latency_valid = 1;
0108 }
0109 }
0110
0111 static void nsim_dev_psample_report_work(struct work_struct *work)
0112 {
0113 struct nsim_dev_psample *psample;
0114 struct psample_metadata md = {};
0115 struct sk_buff *skb;
0116 unsigned long delay;
0117
0118 psample = container_of(work, struct nsim_dev_psample, psample_dw.work);
0119
0120 skb = nsim_dev_psample_skb_build();
0121 if (!skb)
0122 goto out;
0123
0124 nsim_dev_psample_md_prepare(psample, &md, skb->len);
0125 psample_sample_packet(psample->group, skb, psample->rate, &md);
0126 consume_skb(skb);
0127
0128 out:
0129 delay = msecs_to_jiffies(NSIM_PSAMPLE_REPORT_INTERVAL_MS);
0130 schedule_delayed_work(&psample->psample_dw, delay);
0131 }
0132
0133 static int nsim_dev_psample_enable(struct nsim_dev *nsim_dev)
0134 {
0135 struct nsim_dev_psample *psample = nsim_dev->psample;
0136 struct devlink *devlink;
0137 unsigned long delay;
0138
0139 if (psample->is_active)
0140 return -EBUSY;
0141
0142 devlink = priv_to_devlink(nsim_dev);
0143 psample->group = psample_group_get(devlink_net(devlink),
0144 psample->group_num);
0145 if (!psample->group)
0146 return -EINVAL;
0147
0148 delay = msecs_to_jiffies(NSIM_PSAMPLE_REPORT_INTERVAL_MS);
0149 schedule_delayed_work(&psample->psample_dw, delay);
0150
0151 psample->is_active = true;
0152
0153 return 0;
0154 }
0155
0156 static int nsim_dev_psample_disable(struct nsim_dev *nsim_dev)
0157 {
0158 struct nsim_dev_psample *psample = nsim_dev->psample;
0159
0160 if (!psample->is_active)
0161 return -EINVAL;
0162
0163 psample->is_active = false;
0164
0165 cancel_delayed_work_sync(&psample->psample_dw);
0166 psample_group_put(psample->group);
0167
0168 return 0;
0169 }
0170
0171 static ssize_t nsim_dev_psample_enable_write(struct file *file,
0172 const char __user *data,
0173 size_t count, loff_t *ppos)
0174 {
0175 struct nsim_dev *nsim_dev = file->private_data;
0176 bool enable;
0177 int err;
0178
0179 err = kstrtobool_from_user(data, count, &enable);
0180 if (err)
0181 return err;
0182
0183 if (enable)
0184 err = nsim_dev_psample_enable(nsim_dev);
0185 else
0186 err = nsim_dev_psample_disable(nsim_dev);
0187
0188 return err ? err : count;
0189 }
0190
0191 static const struct file_operations nsim_psample_enable_fops = {
0192 .open = simple_open,
0193 .write = nsim_dev_psample_enable_write,
0194 .llseek = generic_file_llseek,
0195 .owner = THIS_MODULE,
0196 };
0197
0198 int nsim_dev_psample_init(struct nsim_dev *nsim_dev)
0199 {
0200 struct nsim_dev_psample *psample;
0201 int err;
0202
0203 psample = kzalloc(sizeof(*psample), GFP_KERNEL);
0204 if (!psample)
0205 return -ENOMEM;
0206 nsim_dev->psample = psample;
0207
0208 INIT_DELAYED_WORK(&psample->psample_dw, nsim_dev_psample_report_work);
0209
0210 psample->ddir = debugfs_create_dir("psample", nsim_dev->ddir);
0211 if (IS_ERR(psample->ddir)) {
0212 err = PTR_ERR(psample->ddir);
0213 goto err_psample_free;
0214 }
0215
0216
0217 psample->rate = 100;
0218 debugfs_create_u32("rate", 0600, psample->ddir, &psample->rate);
0219
0220 psample->group_num = 10;
0221 debugfs_create_u32("group_num", 0600, psample->ddir,
0222 &psample->group_num);
0223
0224 psample->trunc_size = 0;
0225 debugfs_create_u32("trunc_size", 0600, psample->ddir,
0226 &psample->trunc_size);
0227
0228 psample->in_ifindex = 1;
0229 debugfs_create_u32("in_ifindex", 0600, psample->ddir,
0230 &psample->in_ifindex);
0231
0232 psample->out_ifindex = 2;
0233 debugfs_create_u32("out_ifindex", 0600, psample->ddir,
0234 &psample->out_ifindex);
0235
0236 psample->out_tc = 0;
0237 debugfs_create_u16("out_tc", 0600, psample->ddir, &psample->out_tc);
0238
0239 psample->out_tc_occ_max = 10000;
0240 debugfs_create_u64("out_tc_occ_max", 0600, psample->ddir,
0241 &psample->out_tc_occ_max);
0242
0243 psample->latency_max = 50;
0244 debugfs_create_u64("latency_max", 0600, psample->ddir,
0245 &psample->latency_max);
0246
0247 debugfs_create_file("enable", 0200, psample->ddir, nsim_dev,
0248 &nsim_psample_enable_fops);
0249
0250 return 0;
0251
0252 err_psample_free:
0253 kfree(nsim_dev->psample);
0254 return err;
0255 }
0256
0257 void nsim_dev_psample_exit(struct nsim_dev *nsim_dev)
0258 {
0259 debugfs_remove_recursive(nsim_dev->psample->ddir);
0260 if (nsim_dev->psample->is_active) {
0261 cancel_delayed_work_sync(&nsim_dev->psample->psample_dw);
0262 psample_group_put(nsim_dev->psample->group);
0263 }
0264 kfree(nsim_dev->psample);
0265 }
