159 lines · c
1// SPDX-License-Identifier: GPL-2.02#include <stdint.h>3#include <stdbool.h>4#include <stddef.h>5 6#include <linux/bpf.h>7#include <linux/stddef.h>8#include <linux/pkt_cls.h>9#include <linux/if_ether.h>10#include <linux/in.h>11#include <linux/ip.h>12#include <linux/ipv6.h>13 14#include <bpf/bpf_helpers.h>15#include <bpf/bpf_endian.h>16 17#ifndef ctx_ptr18# define ctx_ptr(field) (void *)(long)(field)19#endif20 21#define AF_INET 222#define AF_INET6 1023 24static __always_inline int fill_fib_params_v4(struct __sk_buff *skb,25 struct bpf_fib_lookup *fib_params)26{27 void *data_end = ctx_ptr(skb->data_end);28 void *data = ctx_ptr(skb->data);29 struct iphdr *ip4h;30 31 if (data + sizeof(struct ethhdr) > data_end)32 return -1;33 34 ip4h = (struct iphdr *)(data + sizeof(struct ethhdr));35 if ((void *)(ip4h + 1) > data_end)36 return -1;37 38 fib_params->family = AF_INET;39 fib_params->tos = ip4h->tos;40 fib_params->l4_protocol = ip4h->protocol;41 fib_params->sport = 0;42 fib_params->dport = 0;43 fib_params->tot_len = bpf_ntohs(ip4h->tot_len);44 fib_params->ipv4_src = ip4h->saddr;45 fib_params->ipv4_dst = ip4h->daddr;46 47 return 0;48}49 50static __always_inline int fill_fib_params_v6(struct __sk_buff *skb,51 struct bpf_fib_lookup *fib_params)52{53 struct in6_addr *src = (struct in6_addr *)fib_params->ipv6_src;54 struct in6_addr *dst = (struct in6_addr *)fib_params->ipv6_dst;55 void *data_end = ctx_ptr(skb->data_end);56 void *data = ctx_ptr(skb->data);57 struct ipv6hdr *ip6h;58 59 if (data + sizeof(struct ethhdr) > data_end)60 return -1;61 62 ip6h = (struct ipv6hdr *)(data + sizeof(struct ethhdr));63 if ((void *)(ip6h + 1) > data_end)64 return -1;65 66 fib_params->family = AF_INET6;67 fib_params->flowinfo = 0;68 fib_params->l4_protocol = ip6h->nexthdr;69 fib_params->sport = 0;70 fib_params->dport = 0;71 fib_params->tot_len = bpf_ntohs(ip6h->payload_len);72 *src = ip6h->saddr;73 *dst = ip6h->daddr;74 75 return 0;76}77 78SEC("tc")79int tc_chk(struct __sk_buff *skb)80{81 void *data_end = ctx_ptr(skb->data_end);82 void *data = ctx_ptr(skb->data);83 __u32 *raw = data;84 85 if (data + sizeof(struct ethhdr) > data_end)86 return TC_ACT_SHOT;87 88 return !raw[0] && !raw[1] && !raw[2] ? TC_ACT_SHOT : TC_ACT_OK;89}90 91static __always_inline int tc_redir(struct __sk_buff *skb)92{93 struct bpf_fib_lookup fib_params = { .ifindex = skb->ingress_ifindex };94 __u8 zero[ETH_ALEN * 2];95 int ret = -1;96 97 switch (skb->protocol) {98 case __bpf_constant_htons(ETH_P_IP):99 ret = fill_fib_params_v4(skb, &fib_params);100 break;101 case __bpf_constant_htons(ETH_P_IPV6):102 ret = fill_fib_params_v6(skb, &fib_params);103 break;104 }105 106 if (ret)107 return TC_ACT_OK;108 109 ret = bpf_fib_lookup(skb, &fib_params, sizeof(fib_params), 0);110 if (ret == BPF_FIB_LKUP_RET_NOT_FWDED || ret < 0)111 return TC_ACT_OK;112 113 __builtin_memset(&zero, 0, sizeof(zero));114 if (bpf_skb_store_bytes(skb, 0, &zero, sizeof(zero), 0) < 0)115 return TC_ACT_SHOT;116 117 if (ret == BPF_FIB_LKUP_RET_NO_NEIGH) {118 struct bpf_redir_neigh nh_params = {};119 120 nh_params.nh_family = fib_params.family;121 __builtin_memcpy(&nh_params.ipv6_nh, &fib_params.ipv6_dst,122 sizeof(nh_params.ipv6_nh));123 124 return bpf_redirect_neigh(fib_params.ifindex, &nh_params,125 sizeof(nh_params), 0);126 127 } else if (ret == BPF_FIB_LKUP_RET_SUCCESS) {128 void *data_end = ctx_ptr(skb->data_end);129 struct ethhdr *eth = ctx_ptr(skb->data);130 131 if (eth + 1 > data_end)132 return TC_ACT_SHOT;133 134 __builtin_memcpy(eth->h_dest, fib_params.dmac, ETH_ALEN);135 __builtin_memcpy(eth->h_source, fib_params.smac, ETH_ALEN);136 137 return bpf_redirect(fib_params.ifindex, 0);138 }139 140 return TC_ACT_SHOT;141}142 143/* these are identical, but keep them separate for compatibility with the144 * section names expected by test_tc_redirect.sh145 */146SEC("tc")147int tc_dst(struct __sk_buff *skb)148{149 return tc_redir(skb);150}151 152SEC("tc")153int tc_src(struct __sk_buff *skb)154{155 return tc_redir(skb);156}157 158char __license[] SEC("license") = "GPL";159