1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
| | /* SPDX-License-Identifier: (GPL-2.0-or-later OR BSD-2-clause) */
/* SPDX-FileCopyrightText: 2019 The xdp-tutorial Authors */
/* SPDX-FileCopyrightText: 2025 Yureka Lilian <yureka@cyberchaos.dev> */
/* Based on: https://github.com/xdp-project/xdp-tutorial/blob/main/common/rewrite_helpers.h */
#ifndef __REWRITE_HELPERS_H
#define __REWRITE_HELPERS_H
#include <bpf/bpf_helpers.h>
#include <bpf/bpf_endian.h>
/* Pops the outermost VLAN tag off the packet. Returns the popped VLAN ID on
* success or negative errno on failure.
*/
static __always_inline int vlan_tag_pop(struct xdp_md *ctx, struct ethhdr *eth)
{
void *data_end = (void *)(long)ctx->data_end;
struct ethhdr eth_cpy;
struct vlan_hdr *vlh;
__be16 h_proto;
int vlid;
if (!proto_is_vlan(eth->h_proto))
return -1;
/* Careful with the parenthesis here */
vlh = (void *)(eth + 1);
/* Still need to do bounds checking */
if ((void *) (vlh + 1) > data_end)
return -1;
/* Save vlan ID for returning, h_proto for updating Ethernet header */
vlid = bpf_ntohs(vlh->h_vlan_TCI);
h_proto = vlh->h_vlan_encapsulated_proto;
/* Make a copy of the outer Ethernet header before we cut it off */
__builtin_memcpy(ð_cpy, eth, sizeof(eth_cpy));
/* Actually adjust the head pointer */
if (bpf_xdp_adjust_head(ctx, (int)sizeof(*vlh)))
return -1;
/* Need to re-evaluate data *and* data_end and do new bounds checking
* after adjusting head
*/
eth = (void *)(long)ctx->data;
data_end = (void *)(long)ctx->data_end;
if ((void *) (eth + 1) > data_end)
return -1;
/* Copy back the old Ethernet header and update the proto type */
__builtin_memcpy(eth, ð_cpy, sizeof(*eth));
eth->h_proto = h_proto;
return vlid;
}
/* Pushes a new VLAN tag after the Ethernet header. Returns 0 on success,
* -1 on failure.
*/
static __always_inline int vlan_tag_push(struct xdp_md *ctx,
struct ethhdr *eth, int vlid)
{
void *data_end = (void *)(long)ctx->data_end;
struct ethhdr eth_cpy;
struct vlan_hdr *vlh;
/* First copy the original Ethernet header */
__builtin_memcpy(ð_cpy, eth, sizeof(eth_cpy));
/* Then add space in front of the packet */
if (bpf_xdp_adjust_head(ctx, 0 - (int)sizeof(*vlh)))
return -1;
/* Need to re-evaluate data_end and data after head adjustment, and
* bounds check, even though we know there is enough space (as we
* increased it).
*/
data_end = (void *)(long)ctx->data_end;
eth = (void *)(long)ctx->data;
if ((void *) (eth + 1) > data_end)
return -1;
/* Copy back Ethernet header in the right place, populate VLAN tag with
* ID and proto, and set outer Ethernet header to VLAN type.
*/
__builtin_memcpy(eth, ð_cpy, sizeof(*eth));
vlh = (void *)(eth + 1);
if ((void *) (vlh + 1) > data_end)
return -1;
vlh->h_vlan_TCI = bpf_htons(vlid);
vlh->h_vlan_encapsulated_proto = eth->h_proto;
eth->h_proto = bpf_htons(ETH_P_8021Q);
return 0;
}
#endif
|