184 lines · c
1// SPDX-License-Identifier: GPL-2.02/* Copyright (c) 2019, Oracle and/or its affiliates. All rights reserved. */3 4#define KBUILD_MODNAME "foo"5#include <stddef.h>6#include <string.h>7#include <linux/bpf.h>8#include <linux/icmp.h>9#include <linux/in.h>10#include <linux/if_ether.h>11#include <linux/if_packet.h>12#include <linux/if_vlan.h>13#include <linux/ip.h>14 15#include <bpf/bpf_helpers.h>16#include <bpf/bpf_endian.h>17 18#include "bpf_compiler.h"19#include "xdping.h"20 21struct {22 __uint(type, BPF_MAP_TYPE_HASH);23 __uint(max_entries, 256);24 __type(key, __u32);25 __type(value, struct pinginfo);26} ping_map SEC(".maps");27 28static __always_inline void swap_src_dst_mac(void *data)29{30 unsigned short *p = data;31 unsigned short dst[3];32 33 dst[0] = p[0];34 dst[1] = p[1];35 dst[2] = p[2];36 p[0] = p[3];37 p[1] = p[4];38 p[2] = p[5];39 p[3] = dst[0];40 p[4] = dst[1];41 p[5] = dst[2];42}43 44static __always_inline __u16 csum_fold_helper(__wsum sum)45{46 sum = (sum & 0xffff) + (sum >> 16);47 return ~((sum & 0xffff) + (sum >> 16));48}49 50static __always_inline __u16 ipv4_csum(void *data_start, int data_size)51{52 __wsum sum;53 54 sum = bpf_csum_diff(0, 0, data_start, data_size, 0);55 return csum_fold_helper(sum);56}57 58#define ICMP_ECHO_LEN 6459 60static __always_inline int icmp_check(struct xdp_md *ctx, int type)61{62 void *data_end = (void *)(long)ctx->data_end;63 void *data = (void *)(long)ctx->data;64 struct ethhdr *eth = data;65 struct icmphdr *icmph;66 struct iphdr *iph;67 68 if (data + sizeof(*eth) + sizeof(*iph) + ICMP_ECHO_LEN > data_end)69 return XDP_PASS;70 71 if (eth->h_proto != bpf_htons(ETH_P_IP))72 return XDP_PASS;73 74 iph = data + sizeof(*eth);75 76 if (iph->protocol != IPPROTO_ICMP)77 return XDP_PASS;78 79 if (bpf_ntohs(iph->tot_len) - sizeof(*iph) != ICMP_ECHO_LEN)80 return XDP_PASS;81 82 icmph = data + sizeof(*eth) + sizeof(*iph);83 84 if (icmph->type != type)85 return XDP_PASS;86 87 return XDP_TX;88}89 90SEC("xdp")91int xdping_client(struct xdp_md *ctx)92{93 void *data = (void *)(long)ctx->data;94 struct pinginfo *pinginfo = NULL;95 struct ethhdr *eth = data;96 struct icmphdr *icmph;97 struct iphdr *iph;98 __u64 recvtime;99 __be32 raddr;100 __be16 seq;101 int ret;102 __u8 i;103 104 ret = icmp_check(ctx, ICMP_ECHOREPLY);105 106 if (ret != XDP_TX)107 return ret;108 109 iph = data + sizeof(*eth);110 icmph = data + sizeof(*eth) + sizeof(*iph);111 raddr = iph->saddr;112 113 /* Record time reply received. */114 recvtime = bpf_ktime_get_ns();115 pinginfo = bpf_map_lookup_elem(&ping_map, &raddr);116 if (!pinginfo || pinginfo->seq != icmph->un.echo.sequence)117 return XDP_PASS;118 119 if (pinginfo->start) {120 __pragma_loop_unroll_full121 for (i = 0; i < XDPING_MAX_COUNT; i++) {122 if (pinginfo->times[i] == 0)123 break;124 }125 /* verifier is fussy here... */126 if (i < XDPING_MAX_COUNT) {127 pinginfo->times[i] = recvtime -128 pinginfo->start;129 pinginfo->start = 0;130 i++;131 }132 /* No more space for values? */133 if (i == pinginfo->count || i == XDPING_MAX_COUNT)134 return XDP_PASS;135 }136 137 /* Now convert reply back into echo request. */138 swap_src_dst_mac(data);139 iph->saddr = iph->daddr;140 iph->daddr = raddr;141 icmph->type = ICMP_ECHO;142 seq = bpf_htons(bpf_ntohs(icmph->un.echo.sequence) + 1);143 icmph->un.echo.sequence = seq;144 icmph->checksum = 0;145 icmph->checksum = ipv4_csum(icmph, ICMP_ECHO_LEN);146 147 pinginfo->seq = seq;148 pinginfo->start = bpf_ktime_get_ns();149 150 return XDP_TX;151}152 153SEC("xdp")154int xdping_server(struct xdp_md *ctx)155{156 void *data = (void *)(long)ctx->data;157 struct ethhdr *eth = data;158 struct icmphdr *icmph;159 struct iphdr *iph;160 __be32 raddr;161 int ret;162 163 ret = icmp_check(ctx, ICMP_ECHO);164 165 if (ret != XDP_TX)166 return ret;167 168 iph = data + sizeof(*eth);169 icmph = data + sizeof(*eth) + sizeof(*iph);170 raddr = iph->saddr;171 172 /* Now convert request into echo reply. */173 swap_src_dst_mac(data);174 iph->saddr = iph->daddr;175 iph->daddr = raddr;176 icmph->type = ICMP_ECHOREPLY;177 icmph->checksum = 0;178 icmph->checksum = ipv4_csum(icmph, ICMP_ECHO_LEN);179 180 return XDP_TX;181}182 183char _license[] SEC("license") = "GPL";184