BadVPN – Blame information for rev 1

Subversion Repositories:
Rev:
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 }