263 lines · c
1#include <stddef.h>2#include <inttypes.h>3#include <errno.h>4#include <linux/seg6_local.h>5#include <linux/bpf.h>6#include <bpf/bpf_helpers.h>7#include <bpf/bpf_endian.h>8 9#include "bpf_compiler.h"10 11/* Packet parsing state machine helpers. */12#define cursor_advance(_cursor, _len) \13 ({ void *_tmp = _cursor; _cursor += _len; _tmp; })14 15#define SR6_FLAG_ALERT (1 << 4)16 17#define BPF_PACKET_HEADER __attribute__((packed))18 19struct ip6_t {20 unsigned int ver:4;21 unsigned int priority:8;22 unsigned int flow_label:20;23 unsigned short payload_len;24 unsigned char next_header;25 unsigned char hop_limit;26 unsigned long long src_hi;27 unsigned long long src_lo;28 unsigned long long dst_hi;29 unsigned long long dst_lo;30} BPF_PACKET_HEADER;31 32struct ip6_addr_t {33 unsigned long long hi;34 unsigned long long lo;35} BPF_PACKET_HEADER;36 37struct ip6_srh_t {38 unsigned char nexthdr;39 unsigned char hdrlen;40 unsigned char type;41 unsigned char segments_left;42 unsigned char first_segment;43 unsigned char flags;44 unsigned short tag;45 46 struct ip6_addr_t segments[0];47} BPF_PACKET_HEADER;48 49struct sr6_tlv_t {50 unsigned char type;51 unsigned char len;52 unsigned char value[0];53} BPF_PACKET_HEADER;54 55static __always_inline struct ip6_srh_t *get_srh(struct __sk_buff *skb)56{57 void *cursor, *data_end;58 struct ip6_srh_t *srh;59 struct ip6_t *ip;60 uint8_t *ipver;61 62 data_end = (void *)(long)skb->data_end;63 cursor = (void *)(long)skb->data;64 ipver = (uint8_t *)cursor;65 66 if ((void *)ipver + sizeof(*ipver) > data_end)67 return NULL;68 69 if ((*ipver >> 4) != 6)70 return NULL;71 72 ip = cursor_advance(cursor, sizeof(*ip));73 if ((void *)ip + sizeof(*ip) > data_end)74 return NULL;75 76 if (ip->next_header != 43)77 return NULL;78 79 srh = cursor_advance(cursor, sizeof(*srh));80 if ((void *)srh + sizeof(*srh) > data_end)81 return NULL;82 83 if (srh->type != 4)84 return NULL;85 86 return srh;87}88 89static __always_inline int update_tlv_pad(struct __sk_buff *skb,90 uint32_t new_pad, uint32_t old_pad,91 uint32_t pad_off)92{93 int err;94 95 if (new_pad != old_pad) {96 err = bpf_lwt_seg6_adjust_srh(skb, pad_off,97 (int) new_pad - (int) old_pad);98 if (err)99 return err;100 }101 102 if (new_pad > 0) {103 char pad_tlv_buf[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,104 0, 0, 0};105 struct sr6_tlv_t *pad_tlv = (struct sr6_tlv_t *) pad_tlv_buf;106 107 pad_tlv->type = SR6_TLV_PADDING;108 pad_tlv->len = new_pad - 2;109 110 err = bpf_lwt_seg6_store_bytes(skb, pad_off,111 (void *)pad_tlv_buf, new_pad);112 if (err)113 return err;114 }115 116 return 0;117}118 119static __always_inline int is_valid_tlv_boundary(struct __sk_buff *skb,120 struct ip6_srh_t *srh,121 uint32_t *tlv_off,122 uint32_t *pad_size,123 uint32_t *pad_off)124{125 uint32_t srh_off, cur_off;126 int offset_valid = 0;127 int err;128 129 srh_off = (char *)srh - (char *)(long)skb->data;130 // cur_off = end of segments, start of possible TLVs131 cur_off = srh_off + sizeof(*srh) +132 sizeof(struct ip6_addr_t) * (srh->first_segment + 1);133 134 *pad_off = 0;135 136 // we can only go as far as ~10 TLVs due to the BPF max stack size137 // workaround: define induction variable "i" as "long" instead138 // of "int" to prevent alu32 sub-register spilling.139 __pragma_loop_no_unroll140 for (long i = 0; i < 100; i++) {141 struct sr6_tlv_t tlv;142 143 if (cur_off == *tlv_off)144 offset_valid = 1;145 146 if (cur_off >= srh_off + ((srh->hdrlen + 1) << 3))147 break;148 149 err = bpf_skb_load_bytes(skb, cur_off, &tlv, sizeof(tlv));150 if (err)151 return err;152 153 if (tlv.type == SR6_TLV_PADDING) {154 *pad_size = tlv.len + sizeof(tlv);155 *pad_off = cur_off;156 157 if (*tlv_off == srh_off) {158 *tlv_off = cur_off;159 offset_valid = 1;160 }161 break;162 163 } else if (tlv.type == SR6_TLV_HMAC) {164 break;165 }166 167 cur_off += sizeof(tlv) + tlv.len;168 } // we reached the padding or HMAC TLVs, or the end of the SRH169 170 if (*pad_off == 0)171 *pad_off = cur_off;172 173 if (*tlv_off == -1)174 *tlv_off = cur_off;175 else if (!offset_valid)176 return -EINVAL;177 178 return 0;179}180 181static __always_inline int add_tlv(struct __sk_buff *skb,182 struct ip6_srh_t *srh, uint32_t tlv_off,183 struct sr6_tlv_t *itlv, uint8_t tlv_size)184{185 uint32_t srh_off = (char *)srh - (char *)(long)skb->data;186 uint8_t len_remaining, new_pad;187 uint32_t pad_off = 0;188 uint32_t pad_size = 0;189 uint32_t partial_srh_len;190 int err;191 192 if (tlv_off != -1)193 tlv_off += srh_off;194 195 if (itlv->type == SR6_TLV_PADDING || itlv->type == SR6_TLV_HMAC)196 return -EINVAL;197 198 err = is_valid_tlv_boundary(skb, srh, &tlv_off, &pad_size, &pad_off);199 if (err)200 return err;201 202 err = bpf_lwt_seg6_adjust_srh(skb, tlv_off, sizeof(*itlv) + itlv->len);203 if (err)204 return err;205 206 err = bpf_lwt_seg6_store_bytes(skb, tlv_off, (void *)itlv, tlv_size);207 if (err)208 return err;209 210 // the following can't be moved inside update_tlv_pad because the211 // bpf verifier has some issues with it212 pad_off += sizeof(*itlv) + itlv->len;213 partial_srh_len = pad_off - srh_off;214 len_remaining = partial_srh_len % 8;215 new_pad = 8 - len_remaining;216 217 if (new_pad == 1) // cannot pad for 1 byte only218 new_pad = 9;219 else if (new_pad == 8)220 new_pad = 0;221 222 return update_tlv_pad(skb, new_pad, pad_size, pad_off);223}224 225// Add an Egress TLV fc00::4, add the flag A,226// and apply End.X action to fc42::1227SEC("lwt_seg6local")228int __add_egr_x(struct __sk_buff *skb)229{230 unsigned long long hi = 0xfc42000000000000;231 unsigned long long lo = 0x1;232 struct ip6_srh_t *srh = get_srh(skb);233 uint8_t new_flags = SR6_FLAG_ALERT;234 struct ip6_addr_t addr;235 int err, offset;236 237 if (srh == NULL)238 return BPF_DROP;239 240 uint8_t tlv[20] = {2, 18, 0, 0, 0xfd, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,241 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x4};242 243 err = add_tlv(skb, srh, (srh->hdrlen+1) << 3,244 (struct sr6_tlv_t *)&tlv, 20);245 if (err)246 return BPF_DROP;247 248 offset = sizeof(struct ip6_t) + offsetof(struct ip6_srh_t, flags);249 err = bpf_lwt_seg6_store_bytes(skb, offset,250 (void *)&new_flags, sizeof(new_flags));251 if (err)252 return BPF_DROP;253 254 addr.lo = bpf_cpu_to_be64(lo);255 addr.hi = bpf_cpu_to_be64(hi);256 err = bpf_lwt_seg6_action(skb, SEG6_LOCAL_ACTION_END_X,257 (void *)&addr, sizeof(addr));258 if (err)259 return BPF_DROP;260 return BPF_REDIRECT;261}262char __license[] SEC("license") = "GPL";263