BadVPN – Blame information for rev 1
?pathlinks?
Rev | Author | Line No. | Line |
---|---|---|---|
1 | office | 1 | /** |
2 | * @file FrameDecider.c |
||
3 | * @author Ambroz Bizjak <ambrop7@gmail.com> |
||
4 | * |
||
5 | * @section LICENSE |
||
6 | * |
||
7 | * Redistribution and use in source and binary forms, with or without |
||
8 | * modification, are permitted provided that the following conditions are met: |
||
9 | * 1. Redistributions of source code must retain the above copyright |
||
10 | * notice, this list of conditions and the following disclaimer. |
||
11 | * 2. Redistributions in binary form must reproduce the above copyright |
||
12 | * notice, this list of conditions and the following disclaimer in the |
||
13 | * documentation and/or other materials provided with the distribution. |
||
14 | * 3. Neither the name of the author nor the |
||
15 | * names of its contributors may be used to endorse or promote products |
||
16 | * derived from this software without specific prior written permission. |
||
17 | * |
||
18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND |
||
19 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED |
||
20 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE |
||
21 | * DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY |
||
22 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES |
||
23 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
||
24 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND |
||
25 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT |
||
26 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS |
||
27 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
||
28 | */ |
||
29 | |||
30 | #include <string.h> |
||
31 | #include <stddef.h> |
||
32 | |||
33 | #include <misc/debug.h> |
||
34 | #include <misc/offset.h> |
||
35 | #include <misc/balloc.h> |
||
36 | #include <misc/ethernet_proto.h> |
||
37 | #include <misc/ipv4_proto.h> |
||
38 | #include <misc/igmp_proto.h> |
||
39 | #include <misc/byteorder.h> |
||
40 | #include <misc/compare.h> |
||
41 | #include <misc/print_macros.h> |
||
42 | |||
43 | #include <client/FrameDecider.h> |
||
44 | |||
45 | #include <generated/blog_channel_FrameDecider.h> |
||
46 | |||
47 | #define DECIDE_STATE_NONE 1 |
||
48 | #define DECIDE_STATE_UNICAST 2 |
||
49 | #define DECIDE_STATE_FLOOD 3 |
||
50 | #define DECIDE_STATE_MULTICAST 4 |
||
51 | |||
52 | #define PeerLog(_o, ...) BLog_LogViaFunc((_o)->logfunc, (_o)->user, BLOG_CURRENT_CHANNEL, __VA_ARGS__) |
||
53 | |||
54 | static int compare_macs (const uint8_t *mac1, const uint8_t *mac2) |
||
55 | { |
||
56 | int c = memcmp(mac1, mac2, 6); |
||
57 | return B_COMPARE(c, 0); |
||
58 | } |
||
59 | |||
60 | #include "FrameDecider_macs_tree.h" |
||
61 | #include <structure/SAvl_impl.h> |
||
62 | |||
63 | #include "FrameDecider_groups_tree.h" |
||
64 | #include <structure/SAvl_impl.h> |
||
65 | |||
66 | #include "FrameDecider_multicast_tree.h" |
||
67 | #include <structure/SAvl_impl.h> |
||
68 | |||
69 | static void add_mac_to_peer (FrameDeciderPeer *o, uint8_t *mac) |
||
70 | { |
||
71 | FrameDecider *d = o->d; |
||
72 | |||
73 | // locate entry in tree |
||
74 | struct _FrameDecider_mac_entry *e_entry = FDMacsTree_LookupExact(&d->macs_tree, 0, mac); |
||
75 | if (e_entry) { |
||
76 | if (e_entry->peer == o) { |
||
77 | // this is our MAC; only move it to the end of the used list |
||
78 | LinkedList1_Remove(&o->mac_entries_used, &e_entry->list_node); |
||
79 | LinkedList1_Append(&o->mac_entries_used, &e_entry->list_node); |
||
80 | return; |
||
81 | } |
||
82 | |||
83 | // some other peer has that MAC; disassociate it |
||
84 | FDMacsTree_Remove(&d->macs_tree, 0, e_entry); |
||
85 | LinkedList1_Remove(&e_entry->peer->mac_entries_used, &e_entry->list_node); |
||
86 | LinkedList1_Append(&e_entry->peer->mac_entries_free, &e_entry->list_node); |
||
87 | } |
||
88 | |||
89 | // aquire MAC address entry, if there are no free ones reuse the oldest used one |
||
90 | LinkedList1Node *list_node; |
||
91 | struct _FrameDecider_mac_entry *entry; |
||
92 | if (list_node = LinkedList1_GetFirst(&o->mac_entries_free)) { |
||
93 | entry = UPPER_OBJECT(list_node, struct _FrameDecider_mac_entry, list_node); |
||
94 | ASSERT(entry->peer == o) |
||
95 | |||
96 | // remove from free |
||
97 | LinkedList1_Remove(&o->mac_entries_free, &entry->list_node); |
||
98 | } else { |
||
99 | list_node = LinkedList1_GetFirst(&o->mac_entries_used); |
||
100 | ASSERT(list_node) |
||
101 | entry = UPPER_OBJECT(list_node, struct _FrameDecider_mac_entry, list_node); |
||
102 | ASSERT(entry->peer == o) |
||
103 | |||
104 | // remove from used |
||
105 | FDMacsTree_Remove(&d->macs_tree, 0, entry); |
||
106 | LinkedList1_Remove(&o->mac_entries_used, &entry->list_node); |
||
107 | } |
||
108 | |||
109 | PeerLog(o, BLOG_INFO, "adding MAC %02"PRIx8":%02"PRIx8":%02"PRIx8":%02"PRIx8":%02"PRIx8":%02"PRIx8"", mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]); |
||
110 | |||
111 | // set MAC in entry |
||
112 | memcpy(entry->mac, mac, sizeof(entry->mac)); |
||
113 | |||
114 | // add to used |
||
115 | LinkedList1_Append(&o->mac_entries_used, &entry->list_node); |
||
116 | int res = FDMacsTree_Insert(&d->macs_tree, 0, entry, NULL); |
||
117 | ASSERT_EXECUTE(res) |
||
118 | } |
||
119 | |||
120 | static uint32_t compute_sig_for_group (uint32_t group) |
||
121 | { |
||
122 | return hton32(ntoh32(group)&0x7FFFFF); |
||
123 | } |
||
124 | |||
125 | static uint32_t compute_sig_for_mac (uint8_t *mac) |
||
126 | { |
||
127 | uint32_t sig; |
||
128 | memcpy(&sig, mac + 2, 4); |
||
129 | sig = hton32(ntoh32(sig)&0x7FFFFF); |
||
130 | return sig; |
||
131 | } |
||
132 | |||
133 | static void add_to_multicast (FrameDecider *d, struct _FrameDecider_group_entry *group_entry) |
||
134 | { |
||
135 | // compute sig |
||
136 | uint32_t sig = compute_sig_for_group(group_entry->group); |
||
137 | |||
138 | struct _FrameDecider_group_entry *master = FDMulticastTree_LookupExact(&d->multicast_tree, 0, sig); |
||
139 | if (master) { |
||
140 | // use existing master |
||
141 | ASSERT(master->is_master) |
||
142 | |||
143 | // set not master |
||
144 | group_entry->is_master = 0; |
||
145 | |||
146 | // insert to list |
||
147 | LinkedList3Node_InitAfter(&group_entry->sig_list_node, &master->sig_list_node); |
||
148 | } else { |
||
149 | // make this entry master |
||
150 | |||
151 | // set master |
||
152 | group_entry->is_master = 1; |
||
153 | |||
154 | // set sig |
||
155 | group_entry->master.sig = sig; |
||
156 | |||
157 | // insert to multicast tree |
||
158 | int res = FDMulticastTree_Insert(&d->multicast_tree, 0, group_entry, NULL); |
||
159 | ASSERT_EXECUTE(res) |
||
160 | |||
161 | // init list node |
||
162 | LinkedList3Node_InitLonely(&group_entry->sig_list_node); |
||
163 | } |
||
164 | } |
||
165 | |||
166 | static void remove_from_multicast (FrameDecider *d, struct _FrameDecider_group_entry *group_entry) |
||
167 | { |
||
168 | // compute sig |
||
169 | uint32_t sig = compute_sig_for_group(group_entry->group); |
||
170 | |||
171 | if (group_entry->is_master) { |
||
172 | // remove master from multicast tree |
||
173 | FDMulticastTree_Remove(&d->multicast_tree, 0, group_entry); |
||
174 | |||
175 | if (!LinkedList3Node_IsLonely(&group_entry->sig_list_node)) { |
||
176 | // at least one more group entry for this sig; make another entry the master |
||
177 | |||
178 | // get an entry |
||
179 | LinkedList3Node *list_node = LinkedList3Node_NextOrPrev(&group_entry->sig_list_node); |
||
180 | struct _FrameDecider_group_entry *newmaster = UPPER_OBJECT(list_node, struct _FrameDecider_group_entry, sig_list_node); |
||
181 | ASSERT(!newmaster->is_master) |
||
182 | |||
183 | // set master |
||
184 | newmaster->is_master = 1; |
||
185 | |||
186 | // set sig |
||
187 | newmaster->master.sig = sig; |
||
188 | |||
189 | // insert to multicast tree |
||
190 | int res = FDMulticastTree_Insert(&d->multicast_tree, 0, newmaster, NULL); |
||
191 | ASSERT_EXECUTE(res) |
||
192 | } |
||
193 | } |
||
194 | |||
195 | // free linked list node |
||
196 | LinkedList3Node_Free(&group_entry->sig_list_node); |
||
197 | } |
||
198 | |||
199 | static void add_group_to_peer (FrameDeciderPeer *o, uint32_t group) |
||
200 | { |
||
201 | FrameDecider *d = o->d; |
||
202 | |||
203 | struct _FrameDecider_group_entry *group_entry = FDGroupsTree_LookupExact(&o->groups_tree, 0, group); |
||
204 | if (group_entry) { |
||
205 | // move to end of used list |
||
206 | LinkedList1_Remove(&o->group_entries_used, &group_entry->list_node); |
||
207 | LinkedList1_Append(&o->group_entries_used, &group_entry->list_node); |
||
208 | } else { |
||
209 | PeerLog(o, BLOG_INFO, "joined group %"PRIu8".%"PRIu8".%"PRIu8".%"PRIu8"", |
||
210 | ((uint8_t *)&group)[0], ((uint8_t *)&group)[1], ((uint8_t *)&group)[2], ((uint8_t *)&group)[3] |
||
211 | ); |
||
212 | |||
213 | // aquire group entry, if there are no free ones reuse the earliest used one |
||
214 | LinkedList1Node *node; |
||
215 | if (node = LinkedList1_GetFirst(&o->group_entries_free)) { |
||
216 | group_entry = UPPER_OBJECT(node, struct _FrameDecider_group_entry, list_node); |
||
217 | |||
218 | // remove from free list |
||
219 | LinkedList1_Remove(&o->group_entries_free, &group_entry->list_node); |
||
220 | } else { |
||
221 | node = LinkedList1_GetFirst(&o->group_entries_used); |
||
222 | ASSERT(node) |
||
223 | group_entry = UPPER_OBJECT(node, struct _FrameDecider_group_entry, list_node); |
||
224 | |||
225 | // remove from multicast |
||
226 | remove_from_multicast(d, group_entry); |
||
227 | |||
228 | // remove from peer's groups tree |
||
229 | FDGroupsTree_Remove(&o->groups_tree, 0, group_entry); |
||
230 | |||
231 | // remove from used list |
||
232 | LinkedList1_Remove(&o->group_entries_used, &group_entry->list_node); |
||
233 | } |
||
234 | |||
235 | // add entry to used list |
||
236 | LinkedList1_Append(&o->group_entries_used, &group_entry->list_node); |
||
237 | |||
238 | // set group address |
||
239 | group_entry->group = group; |
||
240 | |||
241 | // insert to peer's groups tree |
||
242 | int res = FDGroupsTree_Insert(&o->groups_tree, 0, group_entry, NULL); |
||
243 | ASSERT_EXECUTE(res) |
||
244 | |||
245 | // add to multicast |
||
246 | add_to_multicast(d, group_entry); |
||
247 | } |
||
248 | |||
249 | // set timer |
||
250 | group_entry->timer_endtime = btime_gettime() + d->igmp_group_membership_interval; |
||
251 | BReactor_SetTimerAbsolute(d->reactor, &group_entry->timer, group_entry->timer_endtime); |
||
252 | } |
||
253 | |||
254 | static void remove_group_entry (struct _FrameDecider_group_entry *group_entry) |
||
255 | { |
||
256 | FrameDeciderPeer *peer = group_entry->peer; |
||
257 | FrameDecider *d = peer->d; |
||
258 | |||
259 | uint32_t group = group_entry->group; |
||
260 | |||
261 | PeerLog(peer, BLOG_INFO, "left group %"PRIu8".%"PRIu8".%"PRIu8".%"PRIu8"", |
||
262 | ((uint8_t *)&group)[0], ((uint8_t *)&group)[1], ((uint8_t *)&group)[2], ((uint8_t *)&group)[3] |
||
263 | ); |
||
264 | |||
265 | // remove from multicast |
||
266 | remove_from_multicast(d, group_entry); |
||
267 | |||
268 | // remove from peer's groups tree |
||
269 | FDGroupsTree_Remove(&peer->groups_tree, 0, group_entry); |
||
270 | |||
271 | // remove from used list |
||
272 | LinkedList1_Remove(&peer->group_entries_used, &group_entry->list_node); |
||
273 | |||
274 | // add to free list |
||
275 | LinkedList1_Append(&peer->group_entries_free, &group_entry->list_node); |
||
276 | |||
277 | // stop timer |
||
278 | BReactor_RemoveTimer(d->reactor, &group_entry->timer); |
||
279 | } |
||
280 | |||
281 | static void lower_group_timers_to_lmqt (FrameDecider *d, uint32_t group) |
||
282 | { |
||
283 | // have to lower all the group timers of this group down to LMQT |
||
284 | |||
285 | // compute sig |
||
286 | uint32_t sig = compute_sig_for_group(group); |
||
287 | |||
288 | // look up the sig in multicast tree |
||
289 | struct _FrameDecider_group_entry *master = FDMulticastTree_LookupExact(&d->multicast_tree, 0, sig); |
||
290 | if (!master) { |
||
291 | return; |
||
292 | } |
||
293 | ASSERT(master->is_master) |
||
294 | |||
295 | // iterate all group entries with this sig |
||
296 | LinkedList3Iterator it; |
||
297 | LinkedList3Iterator_Init(&it, LinkedList3Node_First(&master->sig_list_node), 1); |
||
298 | LinkedList3Node *sig_list_node; |
||
299 | while (sig_list_node = LinkedList3Iterator_Next(&it)) { |
||
300 | struct _FrameDecider_group_entry *group_entry = UPPER_OBJECT(sig_list_node, struct _FrameDecider_group_entry, sig_list_node); |
||
301 | |||
302 | // skip wrong groups |
||
303 | if (group_entry->group != group) { |
||
304 | continue; |
||
305 | } |
||
306 | |||
307 | // lower timer down to LMQT |
||
308 | btime_t now = btime_gettime(); |
||
309 | if (group_entry->timer_endtime > now + d->igmp_last_member_query_time) { |
||
310 | group_entry->timer_endtime = now + d->igmp_last_member_query_time; |
||
311 | BReactor_SetTimerAbsolute(d->reactor, &group_entry->timer, group_entry->timer_endtime); |
||
312 | } |
||
313 | } |
||
314 | } |
||
315 | |||
316 | static void group_entry_timer_handler (struct _FrameDecider_group_entry *group_entry) |
||
317 | { |
||
318 | DebugObject_Access(&group_entry->peer->d_obj); |
||
319 | |||
320 | remove_group_entry(group_entry); |
||
321 | } |
||
322 | |||
323 | void FrameDecider_Init (FrameDecider *o, int max_peer_macs, int max_peer_groups, btime_t igmp_group_membership_interval, btime_t igmp_last_member_query_time, BReactor *reactor) |
||
324 | { |
||
325 | ASSERT(max_peer_macs > 0) |
||
326 | ASSERT(max_peer_groups > 0) |
||
327 | |||
328 | // init arguments |
||
329 | o->max_peer_macs = max_peer_macs; |
||
330 | o->max_peer_groups = max_peer_groups; |
||
331 | o->igmp_group_membership_interval = igmp_group_membership_interval; |
||
332 | o->igmp_last_member_query_time = igmp_last_member_query_time; |
||
333 | o->reactor = reactor; |
||
334 | |||
335 | // init peers list |
||
336 | LinkedList1_Init(&o->peers_list); |
||
337 | |||
338 | // init MAC tree |
||
339 | FDMacsTree_Init(&o->macs_tree); |
||
340 | |||
341 | // init multicast tree |
||
342 | FDMulticastTree_Init(&o->multicast_tree); |
||
343 | |||
344 | // init decide state |
||
345 | o->decide_state = DECIDE_STATE_NONE; |
||
346 | |||
347 | // set no current flood peer |
||
348 | o->decide_flood_current = NULL; |
||
349 | |||
350 | DebugObject_Init(&o->d_obj); |
||
351 | } |
||
352 | |||
353 | void FrameDecider_Free (FrameDecider *o) |
||
354 | { |
||
355 | ASSERT(FDMulticastTree_IsEmpty(&o->multicast_tree)) |
||
356 | ASSERT(FDMacsTree_IsEmpty(&o->macs_tree)) |
||
357 | ASSERT(LinkedList1_IsEmpty(&o->peers_list)) |
||
358 | DebugObject_Free(&o->d_obj); |
||
359 | } |
||
360 | |||
361 | void FrameDecider_AnalyzeAndDecide (FrameDecider *o, const uint8_t *frame, int frame_len) |
||
362 | { |
||
363 | ASSERT(frame_len >= 0) |
||
364 | DebugObject_Access(&o->d_obj); |
||
365 | |||
366 | // reset decide state |
||
367 | switch (o->decide_state) { |
||
368 | case DECIDE_STATE_NONE: |
||
369 | break; |
||
370 | case DECIDE_STATE_UNICAST: |
||
371 | break; |
||
372 | case DECIDE_STATE_FLOOD: |
||
373 | break; |
||
374 | case DECIDE_STATE_MULTICAST: |
||
375 | LinkedList3Iterator_Free(&o->decide_multicast_it); |
||
376 | return; |
||
377 | default: |
||
378 | ASSERT(0); |
||
379 | } |
||
380 | o->decide_state = DECIDE_STATE_NONE; |
||
381 | o->decide_flood_current = NULL; |
||
382 | |||
383 | // analyze frame |
||
384 | |||
385 | const uint8_t *pos = frame; |
||
386 | int len = frame_len; |
||
387 | |||
388 | if (len < sizeof(struct ethernet_header)) { |
||
389 | return; |
||
390 | } |
||
391 | struct ethernet_header eh; |
||
392 | memcpy(&eh, pos, sizeof(eh)); |
||
393 | pos += sizeof(struct ethernet_header); |
||
394 | len -= sizeof(struct ethernet_header); |
||
395 | |||
396 | int is_igmp = 0; |
||
397 | |||
398 | switch (ntoh16(eh.type)) { |
||
399 | case ETHERTYPE_IPV4: { |
||
400 | // check IPv4 header |
||
401 | struct ipv4_header ipv4_header; |
||
402 | if (!ipv4_check((uint8_t *)pos, len, &ipv4_header, (uint8_t **)&pos, &len)) { |
||
403 | BLog(BLOG_INFO, "decide: wrong IP packet"); |
||
404 | goto out; |
||
405 | } |
||
406 | |||
407 | // check if it's IGMP |
||
408 | if (ntoh8(ipv4_header.protocol) != IPV4_PROTOCOL_IGMP) { |
||
409 | goto out; |
||
410 | } |
||
411 | |||
412 | // remember that it's IGMP; we have to flood IGMP frames |
||
413 | is_igmp = 1; |
||
414 | |||
415 | // check IGMP header |
||
416 | if (len < sizeof(struct igmp_base)) { |
||
417 | BLog(BLOG_INFO, "decide: IGMP: short packet"); |
||
418 | goto out; |
||
419 | } |
||
420 | struct igmp_base igmp_base; |
||
421 | memcpy(&igmp_base, pos, sizeof(igmp_base)); |
||
422 | pos += sizeof(struct igmp_base); |
||
423 | len -= sizeof(struct igmp_base); |
||
424 | |||
425 | switch (ntoh8(igmp_base.type)) { |
||
426 | case IGMP_TYPE_MEMBERSHIP_QUERY: { |
||
427 | if (len == sizeof(struct igmp_v2_extra) && ntoh8(igmp_base.max_resp_code) != 0) { |
||
428 | // V2 query |
||
429 | struct igmp_v2_extra query; |
||
430 | memcpy(&query, pos, sizeof(query)); |
||
431 | pos += sizeof(struct igmp_v2_extra); |
||
432 | len -= sizeof(struct igmp_v2_extra); |
||
433 | |||
434 | if (ntoh32(query.group) != 0) { |
||
435 | // got a Group-Specific Query, lower group timers to LMQT |
||
436 | lower_group_timers_to_lmqt(o, query.group); |
||
437 | } |
||
438 | } |
||
439 | else if (len >= sizeof(struct igmp_v3_query_extra)) { |
||
440 | // V3 query |
||
441 | struct igmp_v3_query_extra query; |
||
442 | memcpy(&query, pos, sizeof(query)); |
||
443 | pos += sizeof(struct igmp_v3_query_extra); |
||
444 | len -= sizeof(struct igmp_v3_query_extra); |
||
445 | |||
446 | // iterate sources |
||
447 | uint16_t num_sources = ntoh16(query.number_of_sources); |
||
448 | int i; |
||
449 | for (i = 0; i < num_sources; i++) { |
||
450 | // check source |
||
451 | if (len < sizeof(struct igmp_source)) { |
||
452 | BLog(BLOG_NOTICE, "decide: IGMP: short source"); |
||
453 | goto out; |
||
454 | } |
||
455 | pos += sizeof(struct igmp_source); |
||
456 | len -= sizeof(struct igmp_source); |
||
457 | } |
||
458 | |||
459 | if (ntoh32(query.group) != 0 && num_sources == 0) { |
||
460 | // got a Group-Specific Query, lower group timers to LMQT |
||
461 | lower_group_timers_to_lmqt(o, query.group); |
||
462 | } |
||
463 | } |
||
464 | } break; |
||
465 | } |
||
466 | } break; |
||
467 | } |
||
468 | |||
469 | out:; |
||
470 | |||
471 | const uint8_t broadcast_mac[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff}; |
||
472 | const uint8_t multicast_mac_header[] = {0x01, 0x00, 0x5e}; |
||
473 | |||
474 | // if it's broadcast or IGMP, flood it |
||
475 | if (is_igmp || !memcmp(eh.dest, broadcast_mac, sizeof(broadcast_mac))) { |
||
476 | o->decide_state = DECIDE_STATE_FLOOD; |
||
477 | o->decide_flood_current = LinkedList1_GetFirst(&o->peers_list); |
||
478 | return; |
||
479 | } |
||
480 | |||
481 | // if it's multicast, forward to all peers with the given sig |
||
482 | if (!memcmp(eh.dest, multicast_mac_header, sizeof(multicast_mac_header))) { |
||
483 | // extract group's sig from destination MAC |
||
484 | uint32_t sig = compute_sig_for_mac(eh.dest); |
||
485 | |||
486 | // look up the sig in multicast tree |
||
487 | struct _FrameDecider_group_entry *master = FDMulticastTree_LookupExact(&o->multicast_tree, 0, sig); |
||
488 | if (master) { |
||
489 | ASSERT(master->is_master) |
||
490 | |||
491 | o->decide_state = DECIDE_STATE_MULTICAST; |
||
492 | LinkedList3Iterator_Init(&o->decide_multicast_it, LinkedList3Node_First(&master->sig_list_node), 1); |
||
493 | } |
||
494 | |||
495 | return; |
||
496 | } |
||
497 | |||
498 | // look for MAC entry |
||
499 | struct _FrameDecider_mac_entry *entry = FDMacsTree_LookupExact(&o->macs_tree, 0, eh.dest); |
||
500 | if (entry) { |
||
501 | o->decide_state = DECIDE_STATE_UNICAST; |
||
502 | o->decide_unicast_peer = entry->peer; |
||
503 | return; |
||
504 | } |
||
505 | |||
506 | // unknown destination MAC, flood |
||
507 | o->decide_state = DECIDE_STATE_FLOOD; |
||
508 | o->decide_flood_current = LinkedList1_GetFirst(&o->peers_list); |
||
509 | return; |
||
510 | } |
||
511 | |||
512 | FrameDeciderPeer * FrameDecider_NextDestination (FrameDecider *o) |
||
513 | { |
||
514 | DebugObject_Access(&o->d_obj); |
||
515 | |||
516 | switch (o->decide_state) { |
||
517 | case DECIDE_STATE_NONE: { |
||
518 | return NULL; |
||
519 | } break; |
||
520 | |||
521 | case DECIDE_STATE_UNICAST: { |
||
522 | o->decide_state = DECIDE_STATE_NONE; |
||
523 | |||
524 | return o->decide_unicast_peer; |
||
525 | } break; |
||
526 | |||
527 | case DECIDE_STATE_FLOOD: { |
||
528 | if (!o->decide_flood_current) { |
||
529 | o->decide_state = DECIDE_STATE_NONE; |
||
530 | return NULL; |
||
531 | } |
||
532 | |||
533 | LinkedList1Node *list_node = o->decide_flood_current; |
||
534 | o->decide_flood_current = LinkedList1Node_Next(o->decide_flood_current); |
||
535 | |||
536 | FrameDeciderPeer *peer = UPPER_OBJECT(list_node, FrameDeciderPeer, list_node); |
||
537 | |||
538 | return peer; |
||
539 | } break; |
||
540 | |||
541 | case DECIDE_STATE_MULTICAST: { |
||
542 | LinkedList3Node *list_node = LinkedList3Iterator_Next(&o->decide_multicast_it); |
||
543 | if (!list_node) { |
||
544 | o->decide_state = DECIDE_STATE_NONE; |
||
545 | return NULL; |
||
546 | } |
||
547 | struct _FrameDecider_group_entry *group_entry = UPPER_OBJECT(list_node, struct _FrameDecider_group_entry, sig_list_node); |
||
548 | |||
549 | return group_entry->peer; |
||
550 | } break; |
||
551 | |||
552 | default: |
||
553 | ASSERT(0); |
||
554 | return NULL; |
||
555 | } |
||
556 | } |
||
557 | |||
558 | int FrameDeciderPeer_Init (FrameDeciderPeer *o, FrameDecider *d, void *user, BLog_logfunc logfunc) |
||
559 | { |
||
560 | // init arguments |
||
561 | o->d = d; |
||
562 | o->user = user; |
||
563 | o->logfunc = logfunc; |
||
564 | |||
565 | // allocate MAC entries |
||
566 | if (!(o->mac_entries = (struct _FrameDecider_mac_entry *)BAllocArray(d->max_peer_macs, sizeof(struct _FrameDecider_mac_entry)))) { |
||
567 | PeerLog(o, BLOG_ERROR, "failed to allocate MAC entries"); |
||
568 | goto fail0; |
||
569 | } |
||
570 | |||
571 | // allocate group entries |
||
572 | if (!(o->group_entries = (struct _FrameDecider_group_entry *)BAllocArray(d->max_peer_groups, sizeof(struct _FrameDecider_group_entry)))) { |
||
573 | PeerLog(o, BLOG_ERROR, "failed to allocate group entries"); |
||
574 | goto fail1; |
||
575 | } |
||
576 | |||
577 | // insert to peers list |
||
578 | LinkedList1_Append(&d->peers_list, &o->list_node); |
||
579 | |||
580 | // init MAC entry lists |
||
581 | LinkedList1_Init(&o->mac_entries_free); |
||
582 | LinkedList1_Init(&o->mac_entries_used); |
||
583 | |||
584 | // initialize MAC entries |
||
585 | for (int i = 0; i < d->max_peer_macs; i++) { |
||
586 | struct _FrameDecider_mac_entry *entry = &o->mac_entries[i]; |
||
587 | |||
588 | // set peer |
||
589 | entry->peer = o; |
||
590 | |||
591 | // insert to free list |
||
592 | LinkedList1_Append(&o->mac_entries_free, &entry->list_node); |
||
593 | } |
||
594 | |||
595 | // init group entry lists |
||
596 | LinkedList1_Init(&o->group_entries_free); |
||
597 | LinkedList1_Init(&o->group_entries_used); |
||
598 | |||
599 | // initialize group entries |
||
600 | for (int i = 0; i < d->max_peer_groups; i++) { |
||
601 | struct _FrameDecider_group_entry *entry = &o->group_entries[i]; |
||
602 | |||
603 | // set peer |
||
604 | entry->peer = o; |
||
605 | |||
606 | // insert to free list |
||
607 | LinkedList1_Append(&o->group_entries_free, &entry->list_node); |
||
608 | |||
609 | // init timer |
||
610 | BTimer_Init(&entry->timer, 0, (BTimer_handler)group_entry_timer_handler, entry); |
||
611 | } |
||
612 | |||
613 | // initialize groups tree |
||
614 | FDGroupsTree_Init(&o->groups_tree); |
||
615 | |||
616 | DebugObject_Init(&o->d_obj); |
||
617 | |||
618 | return 1; |
||
619 | |||
620 | fail1: |
||
621 | BFree(o->mac_entries); |
||
622 | fail0: |
||
623 | return 0; |
||
624 | } |
||
625 | |||
626 | void FrameDeciderPeer_Free (FrameDeciderPeer *o) |
||
627 | { |
||
628 | DebugObject_Free(&o->d_obj); |
||
629 | |||
630 | FrameDecider *d = o->d; |
||
631 | |||
632 | // remove decide unicast reference |
||
633 | if (d->decide_state == DECIDE_STATE_UNICAST && d->decide_unicast_peer == o) { |
||
634 | d->decide_state = DECIDE_STATE_NONE; |
||
635 | } |
||
636 | |||
637 | LinkedList1Node *node; |
||
638 | |||
639 | // free group entries |
||
640 | for (node = LinkedList1_GetFirst(&o->group_entries_used); node; node = LinkedList1Node_Next(node)) { |
||
641 | struct _FrameDecider_group_entry *entry = UPPER_OBJECT(node, struct _FrameDecider_group_entry, list_node); |
||
642 | |||
643 | // remove from multicast |
||
644 | remove_from_multicast(d, entry); |
||
645 | |||
646 | // stop timer |
||
647 | BReactor_RemoveTimer(d->reactor, &entry->timer); |
||
648 | } |
||
649 | |||
650 | // remove used MAC entries from tree |
||
651 | for (node = LinkedList1_GetFirst(&o->mac_entries_used); node; node = LinkedList1Node_Next(node)) { |
||
652 | struct _FrameDecider_mac_entry *entry = UPPER_OBJECT(node, struct _FrameDecider_mac_entry, list_node); |
||
653 | |||
654 | // remove from tree |
||
655 | FDMacsTree_Remove(&d->macs_tree, 0, entry); |
||
656 | } |
||
657 | |||
658 | // remove from peers list |
||
659 | if (d->decide_flood_current == &o->list_node) { |
||
660 | d->decide_flood_current = LinkedList1Node_Next(d->decide_flood_current); |
||
661 | } |
||
662 | LinkedList1_Remove(&d->peers_list, &o->list_node); |
||
663 | |||
664 | // free group entries |
||
665 | BFree(o->group_entries); |
||
666 | |||
667 | // free MAC entries |
||
668 | BFree(o->mac_entries); |
||
669 | } |
||
670 | |||
671 | void FrameDeciderPeer_Analyze (FrameDeciderPeer *o, const uint8_t *frame, int frame_len) |
||
672 | { |
||
673 | ASSERT(frame_len >= 0) |
||
674 | DebugObject_Access(&o->d_obj); |
||
675 | |||
676 | const uint8_t *pos = frame; |
||
677 | int len = frame_len; |
||
678 | |||
679 | if (len < sizeof(struct ethernet_header)) { |
||
680 | goto out; |
||
681 | } |
||
682 | struct ethernet_header eh; |
||
683 | memcpy(&eh, pos, sizeof(eh)); |
||
684 | pos += sizeof(struct ethernet_header); |
||
685 | len -= sizeof(struct ethernet_header); |
||
686 | |||
687 | // register source MAC address with this peer |
||
688 | add_mac_to_peer(o, eh.source); |
||
689 | |||
690 | switch (ntoh16(eh.type)) { |
||
691 | case ETHERTYPE_IPV4: { |
||
692 | // check IPv4 header |
||
693 | struct ipv4_header ipv4_header; |
||
694 | if (!ipv4_check((uint8_t *)pos, len, &ipv4_header, (uint8_t **)&pos, &len)) { |
||
695 | PeerLog(o, BLOG_INFO, "analyze: wrong IP packet"); |
||
696 | goto out; |
||
697 | } |
||
698 | |||
699 | // check if it's IGMP |
||
700 | if (ntoh8(ipv4_header.protocol) != IPV4_PROTOCOL_IGMP) { |
||
701 | goto out; |
||
702 | } |
||
703 | |||
704 | // check IGMP header |
||
705 | if (len < sizeof(struct igmp_base)) { |
||
706 | PeerLog(o, BLOG_INFO, "analyze: IGMP: short packet"); |
||
707 | goto out; |
||
708 | } |
||
709 | struct igmp_base igmp_base; |
||
710 | memcpy(&igmp_base, pos, sizeof(igmp_base)); |
||
711 | pos += sizeof(struct igmp_base); |
||
712 | len -= sizeof(struct igmp_base); |
||
713 | |||
714 | switch (ntoh8(igmp_base.type)) { |
||
715 | case IGMP_TYPE_V2_MEMBERSHIP_REPORT: { |
||
716 | // check extra |
||
717 | if (len < sizeof(struct igmp_v2_extra)) { |
||
718 | PeerLog(o, BLOG_INFO, "analyze: IGMP: short v2 report"); |
||
719 | goto out; |
||
720 | } |
||
721 | struct igmp_v2_extra report; |
||
722 | memcpy(&report, pos, sizeof(report)); |
||
723 | pos += sizeof(struct igmp_v2_extra); |
||
724 | len -= sizeof(struct igmp_v2_extra); |
||
725 | |||
726 | // add to group |
||
727 | add_group_to_peer(o, report.group); |
||
728 | } break; |
||
729 | |||
730 | case IGMP_TYPE_V3_MEMBERSHIP_REPORT: { |
||
731 | // check extra |
||
732 | if (len < sizeof(struct igmp_v3_report_extra)) { |
||
733 | PeerLog(o, BLOG_INFO, "analyze: IGMP: short v3 report"); |
||
734 | goto out; |
||
735 | } |
||
736 | struct igmp_v3_report_extra report; |
||
737 | memcpy(&report, pos, sizeof(report)); |
||
738 | pos += sizeof(struct igmp_v3_report_extra); |
||
739 | len -= sizeof(struct igmp_v3_report_extra); |
||
740 | |||
741 | // iterate records |
||
742 | uint16_t num_records = ntoh16(report.number_of_group_records); |
||
743 | for (int i = 0; i < num_records; i++) { |
||
744 | // check record |
||
745 | if (len < sizeof(struct igmp_v3_report_record)) { |
||
746 | PeerLog(o, BLOG_INFO, "analyze: IGMP: short record header"); |
||
747 | goto out; |
||
748 | } |
||
749 | struct igmp_v3_report_record record; |
||
750 | memcpy(&record, pos, sizeof(record)); |
||
751 | pos += sizeof(struct igmp_v3_report_record); |
||
752 | len -= sizeof(struct igmp_v3_report_record); |
||
753 | |||
754 | // iterate sources |
||
755 | uint16_t num_sources = ntoh16(record.number_of_sources); |
||
756 | int j; |
||
757 | for (j = 0; j < num_sources; j++) { |
||
758 | // check source |
||
759 | if (len < sizeof(struct igmp_source)) { |
||
760 | PeerLog(o, BLOG_INFO, "analyze: IGMP: short source"); |
||
761 | goto out; |
||
762 | } |
||
763 | pos += sizeof(struct igmp_source); |
||
764 | len -= sizeof(struct igmp_source); |
||
765 | } |
||
766 | |||
767 | // check aux data |
||
768 | uint16_t aux_len = ntoh16(record.aux_data_len); |
||
769 | if (len < aux_len) { |
||
770 | PeerLog(o, BLOG_INFO, "analyze: IGMP: short record aux data"); |
||
771 | goto out; |
||
772 | } |
||
773 | pos += aux_len; |
||
774 | len -= aux_len; |
||
775 | |||
776 | switch (record.type) { |
||
777 | case IGMP_RECORD_TYPE_MODE_IS_INCLUDE: |
||
778 | case IGMP_RECORD_TYPE_CHANGE_TO_INCLUDE_MODE: |
||
779 | if (num_sources != 0) { |
||
780 | add_group_to_peer(o, record.group); |
||
781 | } |
||
782 | break; |
||
783 | case IGMP_RECORD_TYPE_MODE_IS_EXCLUDE: |
||
784 | case IGMP_RECORD_TYPE_CHANGE_TO_EXCLUDE_MODE: |
||
785 | add_group_to_peer(o, record.group); |
||
786 | break; |
||
787 | } |
||
788 | } |
||
789 | } break; |
||
790 | } |
||
791 | } break; |
||
792 | } |
||
793 | |||
794 | out:; |
||
795 | } |