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