1 /*
2 * Copyright (C) 2021 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 #include <linux/types.h>
18 #include <linux/bpf.h>
19 #include <linux/if_packet.h>
20 #include <linux/ip.h>
21 #include <linux/ipv6.h>
22 #include <linux/if_ether.h>
23 #include <linux/pkt_cls.h>
24 #include <linux/tcp.h>
25 #include <stdint.h>
26 #include <netinet/in.h>
27 #include <netinet/udp.h>
28 #include <string.h>
29
30 // The resulting .o needs to load on the Android T beta 3 bpfloader
31 #define BPFLOADER_MIN_VER BPFLOADER_T_BETA3_VERSION
32
33 #include "bpf_helpers.h"
34 #include "dscp_policy.h"
35
36 DEFINE_BPF_MAP_GRW(switch_comp_map, ARRAY, int, uint64_t, 1, AID_SYSTEM)
37
DEFINE_BPF_MAP_GRW(ipv4_socket_to_policies_map_A,HASH,uint64_t,RuleEntry,MAX_POLICIES,AID_SYSTEM)38 DEFINE_BPF_MAP_GRW(ipv4_socket_to_policies_map_A, HASH, uint64_t, RuleEntry, MAX_POLICIES,
39 AID_SYSTEM)
40 DEFINE_BPF_MAP_GRW(ipv4_socket_to_policies_map_B, HASH, uint64_t, RuleEntry, MAX_POLICIES,
41 AID_SYSTEM)
42 DEFINE_BPF_MAP_GRW(ipv6_socket_to_policies_map_A, HASH, uint64_t, RuleEntry, MAX_POLICIES,
43 AID_SYSTEM)
44 DEFINE_BPF_MAP_GRW(ipv6_socket_to_policies_map_B, HASH, uint64_t, RuleEntry, MAX_POLICIES,
45 AID_SYSTEM)
46
47 DEFINE_BPF_MAP_GRW(ipv4_dscp_policies_map, ARRAY, uint32_t, DscpPolicy, MAX_POLICIES,
48 AID_SYSTEM)
49 DEFINE_BPF_MAP_GRW(ipv6_dscp_policies_map, ARRAY, uint32_t, DscpPolicy, MAX_POLICIES,
50 AID_SYSTEM)
51
52 static inline __always_inline void match_policy(struct __sk_buff* skb, bool ipv4, bool is_eth) {
53 void* data = (void*)(long)skb->data;
54 const void* data_end = (void*)(long)skb->data_end;
55
56 const int l2_header_size = is_eth ? sizeof(struct ethhdr) : 0;
57 struct ethhdr* eth = is_eth ? data : NULL;
58
59 if (data + l2_header_size > data_end) return;
60
61 int zero = 0;
62 int hdr_size = 0;
63 uint64_t* selectedMap = bpf_switch_comp_map_lookup_elem(&zero);
64
65 // use this with HASH map so map lookup only happens once policies have been added?
66 if (!selectedMap) {
67 return;
68 }
69
70 // used for map lookup
71 uint64_t cookie = bpf_get_socket_cookie(skb);
72 if (!cookie)
73 return;
74
75 uint16_t sport = 0;
76 uint16_t dport = 0;
77 uint8_t protocol = 0; // TODO: Use are reserved value? Or int (-1) and cast to uint below?
78 struct in6_addr srcIp = {};
79 struct in6_addr dstIp = {};
80 uint8_t tos = 0; // Only used for IPv4
81 uint8_t priority = 0; // Only used for IPv6
82 uint8_t flow_lbl = 0; // Only used for IPv6
83 if (ipv4) {
84 const struct iphdr* const iph = is_eth ? (void*)(eth + 1) : data;
85 // Must have ipv4 header
86 if (data + l2_header_size + sizeof(*iph) > data_end) return;
87
88 // IP version must be 4
89 if (iph->version != 4) return;
90
91 // We cannot handle IP options, just standard 20 byte == 5 dword minimal IPv4 header
92 if (iph->ihl != 5) return;
93
94 // V4 mapped address in in6_addr sets 10/11 position to 0xff.
95 srcIp.s6_addr32[2] = htonl(0x0000ffff);
96 dstIp.s6_addr32[2] = htonl(0x0000ffff);
97
98 // Copy IPv4 address into in6_addr for easy comparison below.
99 srcIp.s6_addr32[3] = iph->saddr;
100 dstIp.s6_addr32[3] = iph->daddr;
101 protocol = iph->protocol;
102 tos = iph->tos;
103 hdr_size = sizeof(struct iphdr);
104 } else {
105 struct ipv6hdr* ip6h = is_eth ? (void*)(eth + 1) : data;
106 // Must have ipv6 header
107 if (data + l2_header_size + sizeof(*ip6h) > data_end) return;
108
109 if (ip6h->version != 6) return;
110
111 srcIp = ip6h->saddr;
112 dstIp = ip6h->daddr;
113 protocol = ip6h->nexthdr;
114 priority = ip6h->priority;
115 flow_lbl = ip6h->flow_lbl[0];
116 hdr_size = sizeof(struct ipv6hdr);
117 }
118
119 switch (protocol) {
120 case IPPROTO_UDP:
121 case IPPROTO_UDPLITE:
122 {
123 struct udphdr *udp;
124 udp = data + hdr_size;
125 if ((void*)(udp + 1) > data_end) return;
126 sport = udp->source;
127 dport = udp->dest;
128 }
129 break;
130 case IPPROTO_TCP:
131 {
132 struct tcphdr *tcp;
133 tcp = data + hdr_size;
134 if ((void*)(tcp + 1) > data_end) return;
135 sport = tcp->source;
136 dport = tcp->dest;
137 }
138 break;
139 default:
140 return;
141 }
142
143 RuleEntry* existingRule;
144 if (ipv4) {
145 if (*selectedMap == MAP_A) {
146 existingRule = bpf_ipv4_socket_to_policies_map_A_lookup_elem(&cookie);
147 } else {
148 existingRule = bpf_ipv4_socket_to_policies_map_B_lookup_elem(&cookie);
149 }
150 } else {
151 if (*selectedMap == MAP_A) {
152 existingRule = bpf_ipv6_socket_to_policies_map_A_lookup_elem(&cookie);
153 } else {
154 existingRule = bpf_ipv6_socket_to_policies_map_B_lookup_elem(&cookie);
155 }
156 }
157
158 if (existingRule && v6_equal(srcIp, existingRule->srcIp) &&
159 v6_equal(dstIp, existingRule->dstIp) &&
160 skb->ifindex == existingRule->ifindex &&
161 ntohs(sport) == htons(existingRule->srcPort) &&
162 ntohs(dport) == htons(existingRule->dstPort) &&
163 protocol == existingRule->proto) {
164 if (ipv4) {
165 int ecn = tos & 3;
166 uint8_t newDscpVal = (existingRule->dscpVal << 2) + ecn;
167 int oldDscpVal = tos >> 2;
168 bpf_l3_csum_replace(skb, 1, oldDscpVal, newDscpVal, sizeof(uint8_t));
169 bpf_skb_store_bytes(skb, 1, &newDscpVal, sizeof(uint8_t), 0);
170 } else {
171 uint8_t new_priority = (existingRule->dscpVal >> 2) + 0x60;
172 uint8_t new_flow_label = ((existingRule->dscpVal & 0xf) << 6) + (priority >> 6);
173 bpf_skb_store_bytes(skb, 0, &new_priority, sizeof(uint8_t), 0);
174 bpf_skb_store_bytes(skb, 1, &new_flow_label, sizeof(uint8_t), 0);
175 }
176 return;
177 }
178
179 // Linear scan ipv4_dscp_policies_map since no stored params match skb.
180 int bestScore = -1;
181 uint32_t bestMatch = 0;
182
183 for (register uint64_t i = 0; i < MAX_POLICIES; i++) {
184 int score = 0;
185 uint8_t tempMask = 0;
186 // Using a uint64 in for loop prevents infinite loop during BPF load,
187 // but the key is uint32, so convert back.
188 uint32_t key = i;
189
190 DscpPolicy* policy;
191 if (ipv4) {
192 policy = bpf_ipv4_dscp_policies_map_lookup_elem(&key);
193 } else {
194 policy = bpf_ipv6_dscp_policies_map_lookup_elem(&key);
195 }
196
197 // If the policy lookup failed, presentFields is 0, or iface index does not match
198 // index on skb buff, then we can continue to next policy.
199 if (!policy || policy->presentFields == 0 || policy->ifindex != skb->ifindex)
200 continue;
201
202 if ((policy->presentFields & SRC_IP_MASK_FLAG) == SRC_IP_MASK_FLAG &&
203 v6_equal(srcIp, policy->srcIp)) {
204 score++;
205 tempMask |= SRC_IP_MASK_FLAG;
206 }
207 if ((policy->presentFields & DST_IP_MASK_FLAG) == DST_IP_MASK_FLAG &&
208 v6_equal(dstIp, policy->dstIp)) {
209 score++;
210 tempMask |= DST_IP_MASK_FLAG;
211 }
212 if ((policy->presentFields & SRC_PORT_MASK_FLAG) == SRC_PORT_MASK_FLAG &&
213 ntohs(sport) == htons(policy->srcPort)) {
214 score++;
215 tempMask |= SRC_PORT_MASK_FLAG;
216 }
217 if ((policy->presentFields & DST_PORT_MASK_FLAG) == DST_PORT_MASK_FLAG &&
218 ntohs(dport) >= htons(policy->dstPortStart) &&
219 ntohs(dport) <= htons(policy->dstPortEnd)) {
220 score++;
221 tempMask |= DST_PORT_MASK_FLAG;
222 }
223 if ((policy->presentFields & PROTO_MASK_FLAG) == PROTO_MASK_FLAG &&
224 protocol == policy->proto) {
225 score++;
226 tempMask |= PROTO_MASK_FLAG;
227 }
228
229 if (score > bestScore && tempMask == policy->presentFields) {
230 bestMatch = i;
231 bestScore = score;
232 }
233 }
234
235 uint8_t new_tos= 0; // Can 0 be used as default forwarding value?
236 uint8_t new_priority = 0;
237 uint8_t new_flow_lbl = 0;
238 if (bestScore > 0) {
239 DscpPolicy* policy;
240 if (ipv4) {
241 policy = bpf_ipv4_dscp_policies_map_lookup_elem(&bestMatch);
242 } else {
243 policy = bpf_ipv6_dscp_policies_map_lookup_elem(&bestMatch);
244 }
245
246 if (policy) {
247 // TODO: if DSCP value is already set ignore?
248 if (ipv4) {
249 int ecn = tos & 3;
250 new_tos = (policy->dscpVal << 2) + ecn;
251 } else {
252 new_priority = (policy->dscpVal >> 2) + 0x60;
253 new_flow_lbl = ((policy->dscpVal & 0xf) << 6) + (flow_lbl >> 6);
254
255 // Set IPv6 curDscp value to stored value and recalulate priority
256 // and flow label during next use.
257 new_tos = policy->dscpVal;
258 }
259 }
260 } else return;
261
262 RuleEntry value = {
263 .srcIp = srcIp,
264 .dstIp = dstIp,
265 .ifindex = skb->ifindex,
266 .srcPort = sport,
267 .dstPort = dport,
268 .proto = protocol,
269 .dscpVal = new_tos,
270 };
271
272 //Update map with new policy.
273 if (ipv4) {
274 if (*selectedMap == MAP_A) {
275 bpf_ipv4_socket_to_policies_map_A_update_elem(&cookie, &value, BPF_ANY);
276 } else {
277 bpf_ipv4_socket_to_policies_map_B_update_elem(&cookie, &value, BPF_ANY);
278 }
279 } else {
280 if (*selectedMap == MAP_A) {
281 bpf_ipv6_socket_to_policies_map_A_update_elem(&cookie, &value, BPF_ANY);
282 } else {
283 bpf_ipv6_socket_to_policies_map_B_update_elem(&cookie, &value, BPF_ANY);
284 }
285 }
286
287 // Need to store bytes after updating map or program will not load.
288 if (ipv4 && new_tos != (tos & 252)) {
289 int oldDscpVal = tos >> 2;
290 bpf_l3_csum_replace(skb, 1, oldDscpVal, new_tos, sizeof(uint8_t));
291 bpf_skb_store_bytes(skb, 1, &new_tos, sizeof(uint8_t), 0);
292 } else if (!ipv4 && (new_priority != priority || new_flow_lbl != flow_lbl)) {
293 bpf_skb_store_bytes(skb, 0, &new_priority, sizeof(uint8_t), 0);
294 bpf_skb_store_bytes(skb, 1, &new_flow_lbl, sizeof(uint8_t), 0);
295 }
296 return;
297 }
298
299 DEFINE_BPF_PROG_KVER("schedcls/set_dscp_ether", AID_ROOT, AID_SYSTEM,
300 schedcls_set_dscp_ether, KVER(5, 15, 0))
301 (struct __sk_buff* skb) {
302
303 if (skb->pkt_type != PACKET_HOST) return TC_ACT_PIPE;
304
305 if (skb->protocol == htons(ETH_P_IP)) {
306 match_policy(skb, true, true);
307 } else if (skb->protocol == htons(ETH_P_IPV6)) {
308 match_policy(skb, false, true);
309 }
310
311 // Always return TC_ACT_PIPE
312 return TC_ACT_PIPE;
313 }
314
315 DEFINE_BPF_PROG_KVER("schedcls/set_dscp_raw_ip", AID_ROOT, AID_SYSTEM,
316 schedcls_set_dscp_raw_ip, KVER(5, 15, 0))
317 (struct __sk_buff* skb) {
318 if (skb->protocol == htons(ETH_P_IP)) {
319 match_policy(skb, true, false);
320 } else if (skb->protocol == htons(ETH_P_IPV6)) {
321 match_policy(skb, false, false);
322 }
323
324 // Always return TC_ACT_PIPE
325 return TC_ACT_PIPE;
326 }
327
328 LICENSE("Apache 2.0");
329 CRITICAL("Connectivity");
330