1 // SPDX-License-Identifier: GPL-2.0 2 /* 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 21 struct { 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 28 static __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 44 static __always_inline __u16 csum_fold_helper(__wsum sum) 45 { 46 sum = (sum & 0xffff) + (sum >> 16); 47 return ~((sum & 0xffff) + (sum >> 16)); 48 } 49 50 static __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 64 59 60 static __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 90 SEC("xdp") 91 int 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_full 121 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 153 SEC("xdp") 154 int 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 183 char _license[] SEC("license") = "GPL"; 184