Statistics
| Branch: | Revision:

iof-bird / bird-2.0.1 / proto / babel / packets.c @ 6b3f1a54

History | View | Annotate | Download (34 KB)

1
/*
2
 *        BIRD -- The Babel protocol
3
 *
4
 *        Copyright (c) 2015--2016 Toke Hoiland-Jorgensen
5
 *         (c) 2016--2017 Ondrej Zajicek <santiago@crfreenet.org>
6
 *        (c) 2016--2017 CZ.NIC z.s.p.o.
7
 *
8
 *        Can be freely distributed and used under the terms of the GNU GPL.
9
 *
10
 *        This file contains the packet and TLV handling code for the protocol.
11
 */
12

    
13
#include "babel.h"
14

    
15

    
16
struct babel_pkt_header {
17
  u8 magic;
18
  u8 version;
19
  u16 length;
20
} PACKED;
21

    
22
struct babel_tlv {
23
  u8 type;
24
  u8 length;
25
  u8 value[0];
26
} PACKED;
27

    
28
struct babel_tlv_ack_req {
29
  u8 type;
30
  u8 length;
31
  u16 reserved;
32
  u16 nonce;
33
  u16 interval;
34
} PACKED;
35

    
36
struct babel_tlv_ack {
37
  u8 type;
38
  u8 length;
39
  u16 nonce;
40
} PACKED;
41

    
42
struct babel_tlv_hello {
43
  u8 type;
44
  u8 length;
45
  u16 flags;
46
  u16 seqno;
47
  u16 interval;
48
} PACKED;
49

    
50
struct babel_tlv_ihu {
51
  u8 type;
52
  u8 length;
53
  u8 ae;
54
  u8 reserved;
55
  u16 rxcost;
56
  u16 interval;
57
  u8 addr[0];
58
} PACKED;
59

    
60
struct babel_tlv_router_id {
61
  u8 type;
62
  u8 length;
63
  u16 reserved;
64
  u64 router_id;
65
} PACKED;
66

    
67
struct babel_tlv_next_hop {
68
  u8 type;
69
  u8 length;
70
  u8 ae;
71
  u8 reserved;
72
  u8 addr[0];
73
} PACKED;
74

    
75
struct babel_tlv_update {
76
  u8 type;
77
  u8 length;
78
  u8 ae;
79
  u8 flags;
80
  u8 plen;
81
  u8 omitted;
82
  u16 interval;
83
  u16 seqno;
84
  u16 metric;
85
  u8 addr[0];
86
} PACKED;
87

    
88
struct babel_tlv_route_request {
89
  u8 type;
90
  u8 length;
91
  u8 ae;
92
  u8 plen;
93
  u8 addr[0];
94
} PACKED;
95

    
96
struct babel_tlv_seqno_request {
97
  u8 type;
98
  u8 length;
99
  u8 ae;
100
  u8 plen;
101
  u16 seqno;
102
  u8 hop_count;
103
  u8 reserved;
104
  u64 router_id;
105
  u8 addr[0];
106
} PACKED;
107

    
108

    
109
/* Hello flags */
110
#define BABEL_HF_UNICAST        0x8000
111

    
112
/* Update flags */
113
#define BABEL_UF_DEF_PREFIX        0x80
114
#define BABEL_UF_ROUTER_ID        0x40
115

    
116

    
117
struct babel_parse_state {
118
  struct babel_proto *proto;
119
  struct babel_iface *ifa;
120
  ip_addr saddr;
121
  ip_addr next_hop_ip4;
122
  ip_addr next_hop_ip6;
123
  u64 router_id;                /* Router ID used in subsequent updates */
124
  u8 def_ip6_prefix[16];        /* Implicit IPv6 prefix in network order */
125
  u8 def_ip4_prefix[4];                /* Implicit IPv4 prefix in network order */
126
  u8 router_id_seen;                /* router_id field is valid */
127
  u8 def_ip6_prefix_seen;        /* def_ip6_prefix is valid */
128
  u8 def_ip4_prefix_seen;        /* def_ip4_prefix is valid */
129
  u8 current_tlv_endpos;        /* End of self-terminating TLVs (offset from start) */
130
};
131

    
132
enum parse_result {
133
  PARSE_SUCCESS,
134
  PARSE_ERROR,
135
  PARSE_IGNORE,
136
};
137

    
138
struct babel_write_state {
139
  u64 router_id;
140
  u8 router_id_seen;
141
  ip_addr next_hop_ip4;
142
  ip_addr next_hop_ip6;
143
  u8 def_ip6_prefix[16];        /* Implicit IPv6 prefix in network order */
144
  u8 def_ip6_pxlen;
145
};
146

    
147

    
148
#define DROP(DSC,VAL) do { err_dsc = DSC; err_val = VAL; goto drop; } while(0)
149
#define DROP1(DSC) do { err_dsc = DSC; goto drop; } while(0)
150
#define LOG_PKT(msg, args...) \
151
  log_rl(&p->log_pkt_tbf, L_REMOTE "%s: " msg, p->p.name, args)
152

    
153
#define FIRST_TLV(p) ((struct babel_tlv *) (((struct babel_pkt_header *) p) + 1))
154
#define NEXT_TLV(t) ((struct babel_tlv *) (((byte *) t) + TLV_LENGTH(t)))
155
#define TLV_LENGTH(t) (t->type == BABEL_TLV_PAD1 ? 1 : t->length + sizeof(struct babel_tlv))
156
#define TLV_OPT_LENGTH(t) (t->length + sizeof(struct babel_tlv) - sizeof(*t))
157
#define TLV_HDR(tlv,t,l) ({ tlv->type = t; tlv->length = l - sizeof(struct babel_tlv); })
158
#define TLV_HDR0(tlv,t) TLV_HDR(tlv, t, tlv_data[t].min_length)
159

    
160
#define NET_SIZE(n) BYTES(net_pxlen(n))
161

    
162
static inline uint
163
bytes_equal(u8 *b1, u8 *b2, uint maxlen)
164
{
165
  uint i;
166
  for (i = 0; (i < maxlen) && (*b1 == *b2); i++, b1++, b2++)
167
    ;
168
  return i;
169
}
170

    
171
static inline uint
172
get_time16(const void *p)
173
{
174
  uint v = get_u16(p) * BABEL_TIME_UNITS;
175
  return MAX(BABEL_MIN_INTERVAL, v);
176
}
177

    
178
static inline void
179
put_time16(void *p, uint v)
180
{
181
  put_u16(p, v / BABEL_TIME_UNITS);
182
}
183

    
184
static inline void
185
read_ip4_px(net_addr *n, const void *p, uint plen)
186
{
187
  ip4_addr addr = {0};
188
  memcpy(&addr, p, BYTES(plen));
189
  net_fill_ip4(n, ip4_ntoh(addr), plen);
190
}
191

    
192
static inline void
193
put_ip4_px(void *p, net_addr *n)
194
{
195
  ip4_addr addr = ip4_hton(net4_prefix(n));
196
  memcpy(p, &addr, NET_SIZE(n));
197
}
198

    
199
static inline void
200
read_ip6_px(net_addr *n, const void *p, uint plen)
201
{
202
  ip6_addr addr = IPA_NONE;
203
  memcpy(&addr, p, BYTES(plen));
204
  net_fill_ip6(n, ip6_ntoh(addr), plen);
205
}
206

    
207
static inline void
208
put_ip6_px(void *p, net_addr *n)
209
{
210
  ip6_addr addr = ip6_hton(net6_prefix(n));
211
  memcpy(p, &addr, NET_SIZE(n));
212
}
213

    
214
static inline ip6_addr
215
get_ip6_ll(const void *p)
216
{
217
  return ip6_build(0xfe800000, 0, get_u32(p+0), get_u32(p+4));
218
}
219

    
220
static inline void
221
put_ip6_ll(void *p, ip6_addr addr)
222
{
223
  put_u32(p+0, _I2(addr));
224
  put_u32(p+4, _I3(addr));
225
}
226

    
227

    
228
/*
229
 *        TLV read/write functions
230
 */
231

    
232
static int babel_read_ack_req(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
233
static int babel_read_hello(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
234
static int babel_read_ihu(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
235
static int babel_read_router_id(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
236
static int babel_read_next_hop(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
237
static int babel_read_update(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
238
static int babel_read_route_request(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
239
static int babel_read_seqno_request(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
240

    
241
static uint babel_write_ack(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, uint max_len);
242
static uint babel_write_hello(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, uint max_len);
243
static uint babel_write_ihu(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, uint max_len);
244
static uint babel_write_update(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, uint max_len);
245
static uint babel_write_route_request(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, uint max_len);
246
static uint babel_write_seqno_request(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, uint max_len);
247

    
248
struct babel_tlv_data {
249
  u8 min_length;
250
  int (*read_tlv)(struct babel_tlv *hdr, union babel_msg *m, struct babel_parse_state *state);
251
  uint (*write_tlv)(struct babel_tlv *hdr, union babel_msg *m, struct babel_write_state *state, uint max_len);
252
  void (*handle_tlv)(union babel_msg *m, struct babel_iface *ifa);
253
};
254

    
255
static const struct babel_tlv_data tlv_data[BABEL_TLV_MAX] = {
256
  [BABEL_TLV_ACK_REQ] = {
257
    sizeof(struct babel_tlv_ack_req),
258
    babel_read_ack_req,
259
    NULL,
260
    babel_handle_ack_req
261
  },
262
  [BABEL_TLV_ACK] = {
263
    sizeof(struct babel_tlv_ack),
264
    NULL,
265
    babel_write_ack,
266
    NULL
267
  },
268
  [BABEL_TLV_HELLO] = {
269
    sizeof(struct babel_tlv_hello),
270
    babel_read_hello,
271
    babel_write_hello,
272
    babel_handle_hello
273
  },
274
  [BABEL_TLV_IHU] = {
275
    sizeof(struct babel_tlv_ihu),
276
    babel_read_ihu,
277
    babel_write_ihu,
278
    babel_handle_ihu
279
  },
280
  [BABEL_TLV_ROUTER_ID] = {
281
    sizeof(struct babel_tlv_router_id),
282
    babel_read_router_id,
283
    NULL,
284
    NULL
285
  },
286
  [BABEL_TLV_NEXT_HOP] = {
287
    sizeof(struct babel_tlv_next_hop),
288
    babel_read_next_hop,
289
    NULL,
290
    NULL
291
  },
292
  [BABEL_TLV_UPDATE] = {
293
    sizeof(struct babel_tlv_update),
294
    babel_read_update,
295
    babel_write_update,
296
    babel_handle_update
297
  },
298
  [BABEL_TLV_ROUTE_REQUEST] = {
299
    sizeof(struct babel_tlv_route_request),
300
    babel_read_route_request,
301
    babel_write_route_request,
302
    babel_handle_route_request
303
  },
304
  [BABEL_TLV_SEQNO_REQUEST] = {
305
    sizeof(struct babel_tlv_seqno_request),
306
    babel_read_seqno_request,
307
    babel_write_seqno_request,
308
    babel_handle_seqno_request
309
  },
310
};
311

    
312
static int
313
babel_read_ack_req(struct babel_tlv *hdr, union babel_msg *m,
314
                   struct babel_parse_state *state)
315
{
316
  struct babel_tlv_ack_req *tlv = (void *) hdr;
317
  struct babel_msg_ack_req *msg = &m->ack_req;
318

    
319
  msg->type = BABEL_TLV_ACK_REQ;
320
  msg->nonce = get_u16(&tlv->nonce);
321
  msg->interval = get_time16(&tlv->interval);
322
  msg->sender = state->saddr;
323

    
324
  if (!msg->interval)
325
    return PARSE_ERROR;
326

    
327
  return PARSE_SUCCESS;
328
}
329

    
330
static uint
331
babel_write_ack(struct babel_tlv *hdr, union babel_msg *m,
332
                struct babel_write_state *state UNUSED, uint max_len UNUSED)
333
{
334
  struct babel_tlv_ack *tlv = (void *) hdr;
335
  struct babel_msg_ack *msg = &m->ack;
336

    
337
  TLV_HDR0(tlv, BABEL_TLV_ACK);
338
  put_u16(&tlv->nonce, msg->nonce);
339

    
340
  return sizeof(struct babel_tlv_ack);
341
}
342

    
343
static int
344
babel_read_hello(struct babel_tlv *hdr, union babel_msg *m,
345
                 struct babel_parse_state *state)
346
{
347
  struct babel_tlv_hello *tlv = (void *) hdr;
348
  struct babel_msg_hello *msg = &m->hello;
349

    
350
  /* We currently don't support unicast Hello */
351
  u16 flags = get_u16(&tlv->flags);
352
  if (flags & BABEL_HF_UNICAST)
353
    return PARSE_IGNORE;
354

    
355
  msg->type = BABEL_TLV_HELLO;
356
  msg->seqno = get_u16(&tlv->seqno);
357
  msg->interval = get_time16(&tlv->interval);
358
  msg->sender = state->saddr;
359

    
360
  return PARSE_SUCCESS;
361
}
362

    
363
static uint
364
babel_write_hello(struct babel_tlv *hdr, union babel_msg *m,
365
                  struct babel_write_state *state UNUSED, uint max_len UNUSED)
366
{
367
  struct babel_tlv_hello *tlv = (void *) hdr;
368
  struct babel_msg_hello *msg = &m->hello;
369

    
370
  TLV_HDR0(tlv, BABEL_TLV_HELLO);
371
  put_u16(&tlv->seqno, msg->seqno);
372
  put_time16(&tlv->interval, msg->interval);
373

    
374
  return sizeof(struct babel_tlv_hello);
375
}
376

    
377
static int
378
babel_read_ihu(struct babel_tlv *hdr, union babel_msg *m,
379
               struct babel_parse_state *state)
380
{
381
  struct babel_tlv_ihu *tlv = (void *) hdr;
382
  struct babel_msg_ihu *msg = &m->ihu;
383

    
384
  msg->type = BABEL_TLV_IHU;
385
  msg->ae = tlv->ae;
386
  msg->rxcost = get_u16(&tlv->rxcost);
387
  msg->interval = get_time16(&tlv->interval);
388
  msg->addr = IPA_NONE;
389
  msg->sender = state->saddr;
390

    
391
  if (msg->ae >= BABEL_AE_MAX)
392
    return PARSE_IGNORE;
393

    
394
  /*
395
   * We only actually read link-local IPs. In every other case, the addr field
396
   * will be 0 but validation will succeed. The handler takes care of these
397
   * cases. We handle them here anyway because we need the length for parsing
398
   * subtlvs.
399
   */
400
  switch (msg->ae)
401
  {
402
  case BABEL_AE_IP4:
403
    if (TLV_OPT_LENGTH(tlv) < 4)
404
      return PARSE_ERROR;
405
    state->current_tlv_endpos += 4;
406
    break;
407

    
408
  case BABEL_AE_IP6:
409
    if (TLV_OPT_LENGTH(tlv) < 16)
410
      return PARSE_ERROR;
411
    state->current_tlv_endpos += 16;
412
    break;
413

    
414
  case BABEL_AE_IP6_LL:
415
    if (TLV_OPT_LENGTH(tlv) < 8)
416
      return PARSE_ERROR;
417

    
418
    msg->addr = ipa_from_ip6(get_ip6_ll(&tlv->addr));
419
    state->current_tlv_endpos += 8;
420
    break;
421
  }
422

    
423
  return PARSE_SUCCESS;
424
}
425

    
426
static uint
427
babel_write_ihu(struct babel_tlv *hdr, union babel_msg *m,
428
                struct babel_write_state *state UNUSED, uint max_len)
429
{
430
  struct babel_tlv_ihu *tlv = (void *) hdr;
431
  struct babel_msg_ihu *msg = &m->ihu;
432

    
433
  if (ipa_is_link_local(msg->addr) && max_len < sizeof(struct babel_tlv_ihu) + 8)
434
    return 0;
435

    
436
  TLV_HDR0(tlv, BABEL_TLV_IHU);
437
  put_u16(&tlv->rxcost, msg->rxcost);
438
  put_time16(&tlv->interval, msg->interval);
439

    
440
  if (!ipa_is_link_local(msg->addr))
441
  {
442
    tlv->ae = BABEL_AE_WILDCARD;
443
    return sizeof(struct babel_tlv_ihu);
444
  }
445
  put_ip6_ll(&tlv->addr, msg->addr);
446
  tlv->ae = BABEL_AE_IP6_LL;
447
  hdr->length += 8;
448
  return sizeof(struct babel_tlv_ihu) + 8;
449
}
450

    
451
static int
452
babel_read_router_id(struct babel_tlv *hdr, union babel_msg *m UNUSED,
453
                     struct babel_parse_state *state)
454
{
455
  struct babel_tlv_router_id *tlv = (void *) hdr;
456

    
457
  state->router_id = get_u64(&tlv->router_id);
458
  state->router_id_seen = 1;
459

    
460
  return PARSE_IGNORE;
461
}
462

    
463
/* This is called directly from babel_write_update() */
464
static uint
465
babel_write_router_id(struct babel_tlv *hdr, u64 router_id,
466
                      struct babel_write_state *state, uint max_len UNUSED)
467
{
468
  struct babel_tlv_router_id *tlv = (void *) hdr;
469

    
470
  /* We still assume that first min_length bytes are available and zeroed */
471

    
472
  TLV_HDR0(tlv, BABEL_TLV_ROUTER_ID);
473
  put_u64(&tlv->router_id, router_id);
474

    
475
  state->router_id = router_id;
476
  state->router_id_seen = 1;
477

    
478
  return sizeof(struct babel_tlv_router_id);
479
}
480

    
481
static int
482
babel_read_next_hop(struct babel_tlv *hdr, union babel_msg *m UNUSED,
483
                    struct babel_parse_state *state)
484
{
485
  struct babel_tlv_next_hop *tlv = (void *) hdr;
486

    
487
  switch (tlv->ae)
488
  {
489
  case BABEL_AE_WILDCARD:
490
    return PARSE_ERROR;
491

    
492
  case BABEL_AE_IP4:
493
    if (TLV_OPT_LENGTH(tlv) < sizeof(ip4_addr))
494
      return PARSE_ERROR;
495

    
496
    state->next_hop_ip4 = ipa_from_ip4(get_ip4(&tlv->addr));
497
    state->current_tlv_endpos += sizeof(ip4_addr);
498
    return PARSE_IGNORE;
499

    
500
  case BABEL_AE_IP6:
501
    if (TLV_OPT_LENGTH(tlv) < sizeof(ip6_addr))
502
      return PARSE_ERROR;
503

    
504
    state->next_hop_ip6 = ipa_from_ip6(get_ip6(&tlv->addr));
505
    state->current_tlv_endpos += sizeof(ip6_addr);
506
    return PARSE_IGNORE;
507

    
508
  case BABEL_AE_IP6_LL:
509
    if (TLV_OPT_LENGTH(tlv) < 8)
510
      return PARSE_ERROR;
511

    
512
    state->next_hop_ip6 = ipa_from_ip6(get_ip6_ll(&tlv->addr));
513
    state->current_tlv_endpos += 8;
514
    return PARSE_IGNORE;
515

    
516
  default:
517
    return PARSE_IGNORE;
518
  }
519

    
520
  return PARSE_IGNORE;
521
}
522

    
523
/* This is called directly from babel_write_update() and returns -1 if a next
524
   hop should be written but there is not enough space. */
525
static int
526
babel_write_next_hop(struct babel_tlv *hdr, ip_addr addr,
527
                     struct babel_write_state *state, uint max_len)
528
{
529
  struct babel_tlv_next_hop *tlv = (void *) hdr;
530

    
531
  if (ipa_zero(addr))
532
  {
533
    /* Should not happen */
534
    return 0;
535
  }
536
  else if (ipa_is_ip4(addr) && !ipa_equal(addr, state->next_hop_ip4))
537
  {
538
    uint len = sizeof(struct babel_tlv_next_hop) + sizeof(ip4_addr);
539
    if (len > max_len)
540
      return -1;
541

    
542
    TLV_HDR(tlv, BABEL_TLV_NEXT_HOP, len);
543

    
544
    tlv->ae = BABEL_AE_IP4;
545
    put_ip4(&tlv->addr, ipa_to_ip4(addr));
546
    state->next_hop_ip4 = addr;
547

    
548
    return len;
549
  }
550
  else if (ipa_is_ip6(addr) && !ipa_equal(addr, state->next_hop_ip6))
551
  {
552
    uint len = sizeof(struct babel_tlv_next_hop) + sizeof(ip6_addr);
553
    if (len > max_len)
554
      return -1;
555

    
556
    TLV_HDR(tlv, BABEL_TLV_NEXT_HOP, len);
557

    
558
    tlv->ae = BABEL_AE_IP6;
559
    put_ip6(&tlv->addr, ipa_to_ip6(addr));
560
    state->next_hop_ip6 = addr;
561

    
562
    return len;
563
  }
564

    
565
  return 0;
566
}
567

    
568
static int
569
babel_read_update(struct babel_tlv *hdr, union babel_msg *m,
570
                  struct babel_parse_state *state)
571
{
572
  struct babel_tlv_update *tlv = (void *) hdr;
573
  struct babel_msg_update *msg = &m->update;
574

    
575
  msg->type = BABEL_TLV_UPDATE;
576
  msg->interval = get_time16(&tlv->interval);
577
  msg->seqno = get_u16(&tlv->seqno);
578
  msg->metric = get_u16(&tlv->metric);
579

    
580
  /* Length of received prefix data without omitted part */
581
  int len = BYTES(tlv->plen) - (int) tlv->omitted;
582
  u8 buf[16] = {};
583

    
584
  if ((len < 0) || ((uint) len > TLV_OPT_LENGTH(tlv)))
585
    return PARSE_ERROR;
586

    
587
  switch (tlv->ae)
588
  {
589
  case BABEL_AE_WILDCARD:
590
    if (tlv->plen > 0)
591
      return PARSE_ERROR;
592

    
593
    if (msg->metric != 65535)
594
      return PARSE_ERROR;
595

    
596
    msg->wildcard = 1;
597
    break;
598

    
599
  case BABEL_AE_IP4:
600
    if (tlv->plen > IP4_MAX_PREFIX_LENGTH)
601
      return PARSE_ERROR;
602

    
603
    /* Cannot omit data if there is no saved prefix */
604
    if (tlv->omitted && !state->def_ip4_prefix_seen)
605
      return PARSE_ERROR;
606

    
607
    /* Update must have next hop, unless it is retraction */
608
    if (ipa_zero(state->next_hop_ip4) && (msg->metric != BABEL_INFINITY))
609
      return PARSE_ERROR;
610

    
611
    /* Merge saved prefix and received prefix parts */
612
    memcpy(buf, state->def_ip4_prefix, tlv->omitted);
613
    memcpy(buf + tlv->omitted, tlv->addr, len);
614

    
615
    ip4_addr prefix4 = get_ip4(buf);
616
    net_fill_ip4(&msg->net, prefix4, tlv->plen);
617

    
618
    if (tlv->flags & BABEL_UF_DEF_PREFIX)
619
    {
620
      put_ip4(state->def_ip4_prefix, prefix4);
621
      state->def_ip4_prefix_seen = 1;
622
    }
623

    
624
    msg->next_hop = state->next_hop_ip4;
625

    
626
    break;
627

    
628
  case BABEL_AE_IP6:
629
    if (tlv->plen > IP6_MAX_PREFIX_LENGTH)
630
      return PARSE_ERROR;
631

    
632
    /* Cannot omit data if there is no saved prefix */
633
    if (tlv->omitted && !state->def_ip6_prefix_seen)
634
      return PARSE_ERROR;
635

    
636
    /* Merge saved prefix and received prefix parts */
637
    memcpy(buf, state->def_ip6_prefix, tlv->omitted);
638
    memcpy(buf + tlv->omitted, tlv->addr, len);
639

    
640
    ip6_addr prefix6 = get_ip6(buf);
641
    net_fill_ip6(&msg->net, prefix6, tlv->plen);
642

    
643
    if (tlv->flags & BABEL_UF_DEF_PREFIX)
644
    {
645
      put_ip6(state->def_ip6_prefix, prefix6);
646
      state->def_ip6_prefix_seen = 1;
647
    }
648

    
649
    if (tlv->flags & BABEL_UF_ROUTER_ID)
650
    {
651
      state->router_id = ((u64) _I2(prefix6)) << 32 | _I3(prefix6);
652
      state->router_id_seen = 1;
653
    }
654

    
655
    msg->next_hop = state->next_hop_ip6;
656

    
657
    break;
658

    
659
  case BABEL_AE_IP6_LL:
660
    /* ??? */
661
    return PARSE_IGNORE;
662

    
663
  default:
664
    return PARSE_IGNORE;
665
  }
666

    
667
  /* Update must have Router ID, unless it is retraction */
668
  if (!state->router_id_seen && (msg->metric != BABEL_INFINITY))
669
  {
670
    DBG("Babel: No router ID seen before update\n");
671
    return PARSE_ERROR;
672
  }
673

    
674
  msg->router_id = state->router_id;
675
  msg->sender = state->saddr;
676
  state->current_tlv_endpos += len;
677

    
678
  return PARSE_SUCCESS;
679
}
680

    
681
static uint
682
babel_write_update(struct babel_tlv *hdr, union babel_msg *m,
683
                   struct babel_write_state *state, uint max_len)
684
{
685
  struct babel_msg_update *msg = &m->update;
686
  uint len0 = 0;
687

    
688
  /*
689
   * When needed, we write Router-ID TLV before Update TLV and return size of
690
   * both of them. There is enough space for the Router-ID TLV, because
691
   * sizeof(struct babel_tlv_router_id) == sizeof(struct babel_tlv_update).
692
   *
693
   * Router ID is not used for retractions, so do not use it in such case.
694
   */
695
  if ((msg->metric < BABEL_INFINITY) &&
696
      (!state->router_id_seen || (msg->router_id != state->router_id)))
697
  {
698
    len0 = babel_write_router_id(hdr, msg->router_id, state, max_len);
699
    hdr = NEXT_TLV(hdr);
700
  }
701

    
702
  /*
703
   * We also may add Next Hop TLV for regular updates. It may fail for not
704
   * enough space or it may be unnecessary as the next hop is the same as the
705
   * last one already announced. So we handle all three cases.
706
   */
707
  if (msg->metric < BABEL_INFINITY)
708
  {
709
    int l = babel_write_next_hop(hdr, msg->next_hop, state, max_len - len0);
710
    if (l < 0)
711
      return 0;
712

    
713
    if (l)
714
    {
715
      len0 += l;
716
      hdr = NEXT_TLV(hdr);
717
    }
718
  }
719

    
720
  struct babel_tlv_update *tlv = (void *) hdr;
721
  uint len = sizeof(struct babel_tlv_update) + NET_SIZE(&msg->net);
722

    
723
  if (len0 + len > max_len)
724
    return 0;
725

    
726
  memset(tlv, 0, sizeof(struct babel_tlv_update));
727
  TLV_HDR(tlv, BABEL_TLV_UPDATE, len);
728

    
729
  if (msg->wildcard)
730
  {
731
    tlv->ae = BABEL_AE_WILDCARD;
732
    tlv->plen = 0;
733
  }
734
  else if (msg->net.type == NET_IP4)
735
  {
736
    tlv->ae = BABEL_AE_IP4;
737
    tlv->plen = net4_pxlen(&msg->net);
738
    put_ip4_px(tlv->addr, &msg->net);
739
  }
740
  else
741
  {
742
    tlv->ae = BABEL_AE_IP6;
743
    tlv->plen = net6_pxlen(&msg->net);
744

    
745
    /* Address compression - omit initial matching bytes */
746
    u8 buf[16], omit;
747
    put_ip6(buf, net6_prefix(&msg->net));
748
    omit = bytes_equal(buf, state->def_ip6_prefix,
749
                       MIN(tlv->plen, state->def_ip6_pxlen) / 8);
750

    
751
    if (omit > 0)
752
    {
753
      memcpy(tlv->addr, buf + omit, NET_SIZE(&msg->net) - omit);
754

    
755
      tlv->omitted = omit;
756
      tlv->length -= omit;
757
      len -= omit;
758
    }
759
    else
760
    {
761
      put_ip6_px(tlv->addr, &msg->net);
762
      tlv->flags |= BABEL_UF_DEF_PREFIX;
763

    
764
      put_ip6(state->def_ip6_prefix, net6_prefix(&msg->net));
765
      state->def_ip6_pxlen = tlv->plen;
766
    }
767
  }
768

    
769
  put_time16(&tlv->interval, msg->interval);
770
  put_u16(&tlv->seqno, msg->seqno);
771
  put_u16(&tlv->metric, msg->metric);
772

    
773
  return len0 + len;
774
}
775

    
776
static int
777
babel_read_route_request(struct babel_tlv *hdr, union babel_msg *m,
778
                         struct babel_parse_state *state UNUSED)
779
{
780
  struct babel_tlv_route_request *tlv = (void *) hdr;
781
  struct babel_msg_route_request *msg = &m->route_request;
782

    
783
  msg->type = BABEL_TLV_ROUTE_REQUEST;
784

    
785
  switch (tlv->ae)
786
  {
787
  case BABEL_AE_WILDCARD:
788
    /* Wildcard requests must have plen 0 */
789
    if (tlv->plen > 0)
790
      return PARSE_ERROR;
791

    
792
    msg->full = 1;
793
    return PARSE_SUCCESS;
794

    
795
  case BABEL_AE_IP4:
796
    if (tlv->plen > IP4_MAX_PREFIX_LENGTH)
797
      return PARSE_ERROR;
798

    
799
    if (TLV_OPT_LENGTH(tlv) < BYTES(tlv->plen))
800
      return PARSE_ERROR;
801

    
802
    read_ip4_px(&msg->net, tlv->addr, tlv->plen);
803
    state->current_tlv_endpos += BYTES(tlv->plen);
804
    return PARSE_SUCCESS;
805

    
806
  case BABEL_AE_IP6:
807
    if (tlv->plen > IP6_MAX_PREFIX_LENGTH)
808
      return PARSE_ERROR;
809

    
810
    if (TLV_OPT_LENGTH(tlv) < BYTES(tlv->plen))
811
      return PARSE_ERROR;
812

    
813
    read_ip6_px(&msg->net, tlv->addr, tlv->plen);
814
    state->current_tlv_endpos += BYTES(tlv->plen);
815
    return PARSE_SUCCESS;
816

    
817
  case BABEL_AE_IP6_LL:
818
    return PARSE_ERROR;
819

    
820
  default:
821
    return PARSE_IGNORE;
822
  }
823

    
824
  return PARSE_IGNORE;
825
}
826

    
827
static uint
828
babel_write_route_request(struct babel_tlv *hdr, union babel_msg *m,
829
                          struct babel_write_state *state UNUSED, uint max_len)
830
{
831
  struct babel_tlv_route_request *tlv = (void *) hdr;
832
  struct babel_msg_route_request *msg = &m->route_request;
833

    
834
  uint len = sizeof(struct babel_tlv_route_request) + NET_SIZE(&msg->net);
835

    
836
  if (len > max_len)
837
    return 0;
838

    
839
  TLV_HDR(tlv, BABEL_TLV_ROUTE_REQUEST, len);
840

    
841
  if (msg->full)
842
  {
843
    tlv->ae = BABEL_AE_WILDCARD;
844
    tlv->plen = 0;
845
  }
846
  else if (msg->net.type == NET_IP4)
847
  {
848
    tlv->ae = BABEL_AE_IP4;
849
    tlv->plen = net4_pxlen(&msg->net);
850
    put_ip4_px(tlv->addr, &msg->net);
851
  }
852
  else
853
  {
854
    tlv->ae = BABEL_AE_IP6;
855
    tlv->plen = net6_pxlen(&msg->net);
856
    put_ip6_px(tlv->addr, &msg->net);
857
  }
858

    
859
  return len;
860
}
861

    
862
static int
863
babel_read_seqno_request(struct babel_tlv *hdr, union babel_msg *m,
864
                         struct babel_parse_state *state)
865
{
866
  struct babel_tlv_seqno_request *tlv = (void *) hdr;
867
  struct babel_msg_seqno_request *msg = &m->seqno_request;
868

    
869
  msg->type = BABEL_TLV_SEQNO_REQUEST;
870
  msg->seqno = get_u16(&tlv->seqno);
871
  msg->hop_count = tlv->hop_count;
872
  msg->router_id = get_u64(&tlv->router_id);
873
  msg->sender = state->saddr;
874

    
875
  if (tlv->hop_count == 0)
876
    return PARSE_ERROR;
877

    
878
  switch (tlv->ae)
879
  {
880
  case BABEL_AE_WILDCARD:
881
    return PARSE_ERROR;
882

    
883
  case BABEL_AE_IP4:
884
    if (tlv->plen > IP4_MAX_PREFIX_LENGTH)
885
      return PARSE_ERROR;
886

    
887
    if (TLV_OPT_LENGTH(tlv) < BYTES(tlv->plen))
888
      return PARSE_ERROR;
889

    
890
    read_ip4_px(&msg->net, tlv->addr, tlv->plen);
891
    state->current_tlv_endpos += BYTES(tlv->plen);
892
    return PARSE_SUCCESS;
893

    
894
  case BABEL_AE_IP6:
895
    if (tlv->plen > IP6_MAX_PREFIX_LENGTH)
896
      return PARSE_ERROR;
897

    
898
    if (TLV_OPT_LENGTH(tlv) < BYTES(tlv->plen))
899
      return PARSE_ERROR;
900

    
901
    read_ip6_px(&msg->net, tlv->addr, tlv->plen);
902
    state->current_tlv_endpos += BYTES(tlv->plen);
903
    return PARSE_SUCCESS;
904

    
905
  case BABEL_AE_IP6_LL:
906
    return PARSE_ERROR;
907

    
908
  default:
909
    return PARSE_IGNORE;
910
  }
911

    
912
  return PARSE_IGNORE;
913
}
914

    
915
static uint
916
babel_write_seqno_request(struct babel_tlv *hdr, union babel_msg *m,
917
                          struct babel_write_state *state UNUSED, uint max_len)
918
{
919
  struct babel_tlv_seqno_request *tlv = (void *) hdr;
920
  struct babel_msg_seqno_request *msg = &m->seqno_request;
921

    
922
  uint len = sizeof(struct babel_tlv_seqno_request) + NET_SIZE(&msg->net);
923

    
924
  if (len > max_len)
925
    return 0;
926

    
927
  TLV_HDR(tlv, BABEL_TLV_SEQNO_REQUEST, len);
928

    
929
  if (msg->net.type == NET_IP4)
930
  {
931
    tlv->ae = BABEL_AE_IP4;
932
    tlv->plen = net4_pxlen(&msg->net);
933
    put_ip4_px(tlv->addr, &msg->net);
934
  }
935
  else
936
  {
937
    tlv->ae = BABEL_AE_IP6;
938
    tlv->plen = net6_pxlen(&msg->net);
939
    put_ip6_px(tlv->addr, &msg->net);
940
  }
941

    
942
  put_u16(&tlv->seqno, msg->seqno);
943
  tlv->hop_count = msg->hop_count;
944
  put_u64(&tlv->router_id, msg->router_id);
945

    
946
  return len;
947
}
948

    
949
static inline int
950
babel_read_subtlvs(struct babel_tlv *hdr,
951
                   union babel_msg *msg UNUSED,
952
                   struct babel_parse_state *state)
953
{
954
  struct babel_tlv *tlv;
955

    
956
  for (tlv = (void *) hdr + state->current_tlv_endpos;
957
       (void *) tlv < (void *) hdr + TLV_LENGTH(hdr);
958
       tlv = NEXT_TLV(tlv))
959
  {
960
    /*
961
     * The subtlv type space is non-contiguous (due to the mandatory bit), so
962
     * use a switch for dispatch instead of the mapping array we use for TLVs
963
     */
964
    switch (tlv->type)
965
    {
966
    case BABEL_SUBTLV_PAD1:
967
    case BABEL_SUBTLV_PADN:
968
      /* FIXME: Framing errors in PADN are silently ignored, see babel_process_packet() */
969
      break;
970

    
971
    default:
972
      /* Unknown mandatory subtlv; PARSE_IGNORE ignores the whole TLV */
973
      if (tlv->type > 128)
974
      {
975
        DBG("Babel: Mandatory subtlv %d found; skipping TLV\n", tlv->type);
976
        return PARSE_IGNORE;
977
      }
978
      break;
979
    }
980
  }
981

    
982
  return PARSE_SUCCESS;
983
}
984

    
985
static inline int
986
babel_read_tlv(struct babel_tlv *hdr,
987
               union babel_msg *msg,
988
               struct babel_parse_state *state)
989
{
990
  if ((hdr->type <= BABEL_TLV_PADN) ||
991
      (hdr->type >= BABEL_TLV_MAX) ||
992
      !tlv_data[hdr->type].read_tlv)
993
    return PARSE_IGNORE;
994

    
995
  if (TLV_LENGTH(hdr) < tlv_data[hdr->type].min_length)
996
    return PARSE_ERROR;
997

    
998
  state->current_tlv_endpos = tlv_data[hdr->type].min_length;
999
  memset(msg, 0, sizeof(*msg));
1000

    
1001
  int res = tlv_data[hdr->type].read_tlv(hdr, msg, state);
1002
  if (res != PARSE_SUCCESS)
1003
    return res;
1004

    
1005
  return babel_read_subtlvs(hdr, msg, state);
1006
}
1007

    
1008
static uint
1009
babel_write_tlv(struct babel_tlv *hdr,
1010
                union babel_msg *msg,
1011
                struct babel_write_state *state,
1012
                uint max_len)
1013
{
1014
  if ((msg->type <= BABEL_TLV_PADN) ||
1015
      (msg->type >= BABEL_TLV_MAX) ||
1016
      !tlv_data[msg->type].write_tlv)
1017
    return 0;
1018

    
1019
  if (tlv_data[msg->type].min_length > max_len)
1020
    return 0;
1021

    
1022
  memset(hdr, 0, tlv_data[msg->type].min_length);
1023
  return tlv_data[msg->type].write_tlv(hdr, msg, state, max_len);
1024
}
1025

    
1026

    
1027
/*
1028
 *        Packet RX/TX functions
1029
 */
1030

    
1031
static int
1032
babel_send_to(struct babel_iface *ifa, ip_addr dest)
1033
{
1034
  sock *sk = ifa->sk;
1035
  struct babel_pkt_header *hdr = (void *) sk->tbuf;
1036
  int len = get_u16(&hdr->length) + sizeof(struct babel_pkt_header);
1037

    
1038
  DBG("Babel: Sending %d bytes to %I\n", len, dest);
1039
  return sk_send_to(sk, len, dest, 0);
1040
}
1041

    
1042
/**
1043
 * babel_write_queue - Write a TLV queue to a transmission buffer
1044
 * @ifa: Interface holding the transmission buffer
1045
 * @queue: TLV queue to write (containing internal-format TLVs)
1046
 *
1047
 * This function writes a packet to the interface transmission buffer with as
1048
 * many TLVs from the &queue as will fit in the buffer. It returns the number of
1049
 * bytes written (NOT counting the packet header). The function is called by
1050
 * babel_send_queue() and babel_send_unicast() to construct packets for
1051
 * transmission, and uses per-TLV helper functions to convert the
1052
 * internal-format TLVs to their wire representations.
1053
 *
1054
 * The TLVs in the queue are freed after they are written to the buffer.
1055
 */
1056
static uint
1057
babel_write_queue(struct babel_iface *ifa, list *queue)
1058
{
1059
  struct babel_proto *p = ifa->proto;
1060
  struct babel_write_state state = { .next_hop_ip6 = ifa->addr };
1061

    
1062
  if (EMPTY_LIST(*queue))
1063
    return 0;
1064

    
1065
  byte *pos = ifa->sk->tbuf;
1066
  byte *end = pos + ifa->tx_length;
1067

    
1068
  struct babel_pkt_header *pkt = (void *) pos;
1069
  pkt->magic = BABEL_MAGIC;
1070
  pkt->version = BABEL_VERSION;
1071
  pkt->length = 0;
1072
  pos += sizeof(struct babel_pkt_header);
1073

    
1074
  struct babel_msg_node *msg;
1075
  WALK_LIST_FIRST(msg, *queue)
1076
  {
1077
    if (pos >= end)
1078
      break;
1079

    
1080
    int len = babel_write_tlv((struct babel_tlv *) pos, &msg->msg, &state, end - pos);
1081

    
1082
    if (!len)
1083
      break;
1084

    
1085
    pos += len;
1086
    rem_node(NODE msg);
1087
    sl_free(p->msg_slab, msg);
1088
  }
1089

    
1090
  uint plen = pos - (byte *) pkt;
1091
  put_u16(&pkt->length, plen - sizeof(struct babel_pkt_header));
1092

    
1093
  return plen;
1094
}
1095

    
1096
void
1097
babel_send_queue(void *arg)
1098
{
1099
  struct babel_iface *ifa = arg;
1100
  while ((babel_write_queue(ifa, &ifa->msg_queue) > 0) &&
1101
         (babel_send_to(ifa, IP6_BABEL_ROUTERS) > 0));
1102
}
1103

    
1104
static inline void
1105
babel_kick_queue(struct babel_iface *ifa)
1106
{
1107
  /*
1108
   * Only schedule send event if there is not already data in the socket buffer.
1109
   * Otherwise we may overwrite the data already in the buffer.
1110
   */
1111

    
1112
  if ((ifa->sk->tpos == ifa->sk->tbuf) && !ev_active(ifa->send_event))
1113
    ev_schedule(ifa->send_event);
1114
}
1115

    
1116
/**
1117
 * babel_send_unicast - send a single TLV via unicast to a destination
1118
 * @msg: TLV to send
1119
 * @ifa: Interface to send via
1120
 * @dest: Destination of the TLV
1121
 *
1122
 * This function is used to send a single TLV via unicast to a designated
1123
 * receiver. This is used for replying to certain incoming requests, and for
1124
 * sending unicast requests to refresh routes before they expire.
1125
 */
1126
void
1127
babel_send_unicast(union babel_msg *msg, struct babel_iface *ifa, ip_addr dest)
1128
{
1129
  struct babel_proto *p = ifa->proto;
1130
  struct babel_msg_node *msgn = sl_alloc(p->msg_slab);
1131
  list queue;
1132

    
1133
  msgn->msg = *msg;
1134
  init_list(&queue);
1135
  add_tail(&queue, NODE msgn);
1136
  babel_write_queue(ifa, &queue);
1137
  babel_send_to(ifa, dest);
1138

    
1139
  /* We could overwrite waiting packet here, we may have to kick TX queue */
1140
  if (!EMPTY_LIST(ifa->msg_queue))
1141
    babel_kick_queue(ifa);
1142
}
1143

    
1144
/**
1145
 * babel_enqueue - enqueue a TLV for transmission on an interface
1146
 * @msg: TLV to enqueue (in internal TLV format)
1147
 * @ifa: Interface to enqueue to
1148
 *
1149
 * This function is called to enqueue a TLV for subsequent transmission on an
1150
 * interface. The transmission event is triggered whenever a TLV is enqueued;
1151
 * this ensures that TLVs will be transmitted in a timely manner, but that TLVs
1152
 * which are enqueued in rapid succession can be transmitted together in one
1153
 * packet.
1154
 */
1155
void
1156
babel_enqueue(union babel_msg *msg, struct babel_iface *ifa)
1157
{
1158
  struct babel_proto *p = ifa->proto;
1159
  struct babel_msg_node *msgn = sl_alloc(p->msg_slab);
1160
  msgn->msg = *msg;
1161
  add_tail(&ifa->msg_queue, NODE msgn);
1162
  babel_kick_queue(ifa);
1163
}
1164

    
1165
/**
1166
 * babel_process_packet - process incoming data packet
1167
 * @pkt: Pointer to the packet data
1168
 * @len: Length of received packet
1169
 * @saddr: Address of packet sender
1170
 * @ifa: Interface packet was received on.
1171
 *
1172
 * This function is the main processing hook of incoming Babel packets. It
1173
 * checks that the packet header is well-formed, then processes the TLVs
1174
 * contained in the packet. This is done in two passes: First all TLVs are
1175
 * parsed into the internal TLV format. If a TLV parser fails, processing of the
1176
 * rest of the packet is aborted.
1177
 *
1178
 * After the parsing step, the TLV handlers are called for each parsed TLV in
1179
 * order.
1180
 */
1181
static void
1182
babel_process_packet(struct babel_pkt_header *pkt, int len,
1183
                     ip_addr saddr, struct babel_iface *ifa)
1184
{
1185
  struct babel_proto *p = ifa->proto;
1186
  struct babel_tlv *tlv;
1187
  struct babel_msg_node *msg;
1188
  list msgs;
1189
  int res;
1190

    
1191
  int plen = sizeof(struct babel_pkt_header) + get_u16(&pkt->length);
1192
  byte *pos;
1193
  byte *end = (byte *)pkt + plen;
1194

    
1195
  struct babel_parse_state state = {
1196
    .proto          = p,
1197
    .ifa          = ifa,
1198
    .saddr          = saddr,
1199
    .next_hop_ip6 = saddr,
1200
  };
1201

    
1202
  if ((pkt->magic != BABEL_MAGIC) || (pkt->version != BABEL_VERSION))
1203
  {
1204
    TRACE(D_PACKETS, "Strange packet from %I via %s - magic %d version %d",
1205
          saddr, ifa->iface->name, pkt->magic, pkt->version);
1206
    return;
1207
  }
1208

    
1209
  if (plen > len)
1210
  {
1211
    LOG_PKT("Bad packet from %I via %s - %s (%u)",
1212
            saddr, ifa->iface->name, "length mismatch", plen);
1213
    return;
1214
  }
1215

    
1216
  TRACE(D_PACKETS, "Packet received from %I via %s",
1217
        saddr, ifa->iface->name);
1218

    
1219
  init_list(&msgs);
1220

    
1221
  /* First pass through the packet TLV by TLV, parsing each into internal data
1222
     structures. */
1223
  for (tlv = FIRST_TLV(pkt);
1224
       (byte *)tlv < end;
1225
       tlv = NEXT_TLV(tlv))
1226
  {
1227
    /* Ugly special case */
1228
    if (tlv->type == BABEL_TLV_PAD1)
1229
      continue;
1230

    
1231
    /* The end of the common TLV header */
1232
    pos = (byte *)tlv + sizeof(struct babel_tlv);
1233
    if ((pos > end) || (pos + tlv->length > end))
1234
    {
1235
      LOG_PKT("Bad TLV from %I via %s type %d pos %d - framing error",
1236
              saddr, ifa->iface->name, tlv->type, (byte *)tlv - (byte *)pkt);
1237
      break;
1238
    }
1239

    
1240
    msg = sl_alloc(p->msg_slab);
1241
    res = babel_read_tlv(tlv, &msg->msg, &state);
1242
    if (res == PARSE_SUCCESS)
1243
    {
1244
      add_tail(&msgs, NODE msg);
1245
    }
1246
    else if (res == PARSE_IGNORE)
1247
    {
1248
      DBG("Babel: Ignoring TLV of type %d\n", tlv->type);
1249
      sl_free(p->msg_slab, msg);
1250
    }
1251
    else /* PARSE_ERROR */
1252
    {
1253
      LOG_PKT("Bad TLV from %I via %s type %d pos %d - parse error",
1254
              saddr, ifa->iface->name, tlv->type, (byte *)tlv - (byte *)pkt);
1255
      sl_free(p->msg_slab, msg);
1256
      break;
1257
    }
1258
  }
1259

    
1260
  /* Parsing done, handle all parsed TLVs */
1261
  WALK_LIST_FIRST(msg, msgs)
1262
  {
1263
    if (tlv_data[msg->msg.type].handle_tlv)
1264
      tlv_data[msg->msg.type].handle_tlv(&msg->msg, ifa);
1265
    rem_node(NODE msg);
1266
    sl_free(p->msg_slab, msg);
1267
  }
1268
}
1269

    
1270
static void
1271
babel_err_hook(sock *sk, int err)
1272
{
1273
  struct babel_iface *ifa = sk->data;
1274
  struct babel_proto *p = ifa->proto;
1275

    
1276
  log(L_ERR "%s: Socket error on %s: %M", p->p.name, ifa->iface->name, err);
1277
  /* FIXME: Drop queued TLVs here? */
1278
}
1279

    
1280

    
1281
static void
1282
babel_tx_hook(sock *sk)
1283
{
1284
  struct babel_iface *ifa = sk->data;
1285

    
1286
  DBG("Babel: TX hook called (iface %s, src %I, dst %I)\n",
1287
      sk->iface->name, sk->saddr, sk->daddr);
1288

    
1289
  babel_send_queue(ifa);
1290
}
1291

    
1292

    
1293
static int
1294
babel_rx_hook(sock *sk, uint len)
1295
{
1296
  struct babel_iface *ifa = sk->data;
1297
  struct babel_proto *p = ifa->proto;
1298
  const char *err_dsc = NULL;
1299
  uint err_val = 0;
1300

    
1301
  if (sk->lifindex != ifa->iface->index)
1302
    return 1;
1303

    
1304
  DBG("Babel: RX hook called (iface %s, src %I, dst %I)\n",
1305
      sk->iface->name, sk->faddr, sk->laddr);
1306

    
1307
  /* Silently ignore my own packets */
1308
  if (ipa_equal(sk->faddr, sk->saddr))
1309
    return 1;
1310

    
1311
  if (!ipa_is_link_local(sk->faddr))
1312
    DROP1("wrong src address");
1313

    
1314
  if (sk->fport != ifa->cf->port)
1315
    DROP("wrong src port", sk->fport);
1316

    
1317
  if (len < sizeof(struct babel_pkt_header))
1318
    DROP("too short", len);
1319

    
1320
  if (sk->flags & SKF_TRUNCATED)
1321
    DROP("truncated", len);
1322

    
1323
  babel_process_packet((struct babel_pkt_header *) sk->rbuf, len, sk->faddr, ifa);
1324
  return 1;
1325

    
1326
drop:
1327
  LOG_PKT("Bad packet from %I via %s - %s (%u)",
1328
          sk->faddr, sk->iface->name, err_dsc, err_val);
1329
  return 1;
1330
}
1331

    
1332
int
1333
babel_open_socket(struct babel_iface *ifa)
1334
{
1335
  struct babel_proto *p = ifa->proto;
1336

    
1337
  sock *sk;
1338
  sk = sk_new(ifa->pool);
1339
  sk->type = SK_UDP;
1340
  sk->sport = ifa->cf->port;
1341
  sk->dport = ifa->cf->port;
1342
  sk->iface = ifa->iface;
1343
  sk->saddr = ifa->addr;
1344
  sk->vrf = p->p.vrf;
1345

    
1346
  sk->rx_hook = babel_rx_hook;
1347
  sk->tx_hook = babel_tx_hook;
1348
  sk->err_hook = babel_err_hook;
1349
  sk->data = ifa;
1350

    
1351
  sk->tos = ifa->cf->tx_tos;
1352
  sk->priority = ifa->cf->tx_priority;
1353
  sk->ttl = 1;
1354
  sk->flags = SKF_LADDR_RX;
1355

    
1356
  if (sk_open(sk) < 0)
1357
    goto err;
1358

    
1359
  if (sk_setup_multicast(sk) < 0)
1360
    goto err;
1361

    
1362
  if (sk_join_group(sk, IP6_BABEL_ROUTERS) < 0)
1363
    goto err;
1364

    
1365
  ifa->sk = sk;
1366
  return 1;
1367

    
1368
err:
1369
  sk_log_error(sk, p->p.name);
1370
  rfree(sk);
1371
  return 0;
1372
}