Statistics
| Branch: | Revision:

iof-bird-daemon / proto / babel / packets.c @ 4324025f

History | View | Annotate | Download (30.9 KB)

1
/*
2
 *        BIRD -- The Babel protocol
3
 *
4
 *        Copyright (c) 2015--2016 Toke Hoiland-Jorgensen
5
 *
6
 *        Can be freely distributed and used under the terms of the GNU GPL.
7
 *
8
 *        This file contains the packet and TLV handling code for the protocol.
9
 */
10

    
11
#include "babel.h"
12

    
13

    
14
struct babel_pkt_header {
15
  u8 magic;
16
  u8 version;
17
  u16 length;
18
} PACKED;
19

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

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

    
34
struct babel_tlv_ack {
35
  u8 type;
36
  u8 length;
37
  u16 nonce;
38
} PACKED;
39

    
40
struct babel_tlv_hello {
41
  u8 type;
42
  u8 length;
43
  u16 reserved;
44
  u16 seqno;
45
  u16 interval;
46
} PACKED;
47

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

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

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

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

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

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

    
106

    
107
#define BABEL_FLAG_DEF_PREFIX        0x80
108
#define BABEL_FLAG_ROUTER_ID        0x40
109

    
110

    
111
struct babel_parse_state {
112
  struct babel_proto *proto;
113
  struct babel_iface *ifa;
114
  ip_addr saddr;
115
  ip_addr next_hop_ip4;
116
  ip_addr next_hop_ip6;
117
  u64 router_id;                /* Router ID used in subsequent updates */
118
  u8 def_ip6_prefix[16];        /* Implicit IPv6 prefix in network order */
119
  u8 def_ip4_prefix[4];                /* Implicit IPv4 prefix in network order */
120
  u8 router_id_seen;                /* router_id field is valid */
121
  u8 def_ip6_prefix_seen;        /* def_ip6_prefix is valid */
122
  u8 def_ip4_prefix_seen;        /* def_ip4_prefix is valid */
123
};
124

    
125
enum parse_result {
126
  PARSE_SUCCESS,
127
  PARSE_ERROR,
128
  PARSE_IGNORE,
129
};
130

    
131
struct babel_write_state {
132
  u64 router_id;
133
  u8 router_id_seen;
134
  ip_addr next_hop_ip4;
135
  ip_addr next_hop_ip6;
136
};
137

    
138

    
139
#define DROP(DSC,VAL) do { err_dsc = DSC; err_val = VAL; goto drop; } while(0)
140
#define DROP1(DSC) do { err_dsc = DSC; goto drop; } while(0)
141
#define LOG_PKT(msg, args...) \
142
  log_rl(&p->log_pkt_tbf, L_REMOTE "%s: " msg, p->p.name, args)
143

    
144
#define FIRST_TLV(p) ((struct babel_tlv *) (((struct babel_pkt_header *) p) + 1))
145
#define NEXT_TLV(t) ((struct babel_tlv *) (((byte *) t) + TLV_LENGTH(t)))
146
#define TLV_LENGTH(t) (t->type == BABEL_TLV_PAD1 ? 1 : t->length + sizeof(struct babel_tlv))
147
#define TLV_OPT_LENGTH(t) (t->length + sizeof(struct babel_tlv) - sizeof(*t))
148
#define TLV_HDR(tlv,t,l) ({ tlv->type = t; tlv->length = l - sizeof(struct babel_tlv); })
149
#define TLV_HDR0(tlv,t) TLV_HDR(tlv, t, tlv_data[t].min_length)
150

    
151
#define NET_SIZE(n) BYTES(net_pxlen(n))
152

    
153

    
154
static inline u16
155
get_time16(const void *p)
156
{
157
  u16 v = get_u16(p) / BABEL_TIME_UNITS;
158
  return MAX(1, v);
159
}
160

    
161
static inline void
162
put_time16(void *p, u16 v)
163
{
164
  put_u16(p, v * BABEL_TIME_UNITS);
165
}
166

    
167
static inline void
168
read_ip4_px(net_addr *n, const void *p, uint plen)
169
{
170
  ip4_addr addr = {0};
171
  memcpy(&addr, p, BYTES(plen));
172
  net_fill_ip4(n, ip4_ntoh(addr), plen);
173
}
174

    
175
static inline void
176
put_ip4_px(void *p, net_addr *n)
177
{
178
  ip4_addr addr = ip4_hton(net4_prefix(n));
179
  memcpy(p, &addr, NET_SIZE(n));
180
}
181

    
182
static inline void
183
read_ip6_px(net_addr *n, const void *p, uint plen)
184
{
185
  ip6_addr addr = IPA_NONE;
186
  memcpy(&addr, p, BYTES(plen));
187
  net_fill_ip6(n, ip6_ntoh(addr), plen);
188
}
189

    
190
static inline void
191
put_ip6_px(void *p, net_addr *n)
192
{
193
  ip6_addr addr = ip6_hton(net6_prefix(n));
194
  memcpy(p, &addr, NET_SIZE(n));
195
}
196

    
197
static inline ip6_addr
198
get_ip6_ll(const void *p)
199
{
200
  return ip6_build(0xfe800000, 0, get_u32(p+0), get_u32(p+4));
201
}
202

    
203
static inline void
204
put_ip6_ll(void *p, ip6_addr addr)
205
{
206
  put_u32(p+0, _I2(addr));
207
  put_u32(p+4, _I3(addr));
208
}
209

    
210

    
211
/*
212
 *        TLV read/write functions
213
 */
214

    
215
static int babel_read_ack_req(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
216
static int babel_read_hello(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
217
static int babel_read_ihu(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
218
static int babel_read_router_id(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
219
static int babel_read_next_hop(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
220
static int babel_read_update(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
221
static int babel_read_route_request(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
222
static int babel_read_seqno_request(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
223

    
224
static uint babel_write_ack(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, uint max_len);
225
static uint babel_write_hello(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, uint max_len);
226
static uint babel_write_ihu(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, uint max_len);
227
static uint babel_write_update(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, uint max_len);
228
static uint babel_write_route_request(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, uint max_len);
229
static uint babel_write_seqno_request(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, uint max_len);
230

    
231
struct babel_tlv_data {
232
  u8 min_length;
233
  int (*read_tlv)(struct babel_tlv *hdr, union babel_msg *m, struct babel_parse_state *state);
234
  uint (*write_tlv)(struct babel_tlv *hdr, union babel_msg *m, struct babel_write_state *state, uint max_len);
235
  void (*handle_tlv)(union babel_msg *m, struct babel_iface *ifa);
236
};
237

    
238
static const struct babel_tlv_data tlv_data[BABEL_TLV_MAX] = {
239
  [BABEL_TLV_ACK_REQ] = {
240
    sizeof(struct babel_tlv_ack_req),
241
    babel_read_ack_req,
242
    NULL,
243
    babel_handle_ack_req
244
  },
245
  [BABEL_TLV_ACK] = {
246
    sizeof(struct babel_tlv_ack),
247
    NULL,
248
    babel_write_ack,
249
    NULL
250
  },
251
  [BABEL_TLV_HELLO] = {
252
    sizeof(struct babel_tlv_hello),
253
    babel_read_hello,
254
    babel_write_hello,
255
    babel_handle_hello
256
  },
257
  [BABEL_TLV_IHU] = {
258
    sizeof(struct babel_tlv_ihu),
259
    babel_read_ihu,
260
    babel_write_ihu,
261
    babel_handle_ihu
262
  },
263
  [BABEL_TLV_ROUTER_ID] = {
264
    sizeof(struct babel_tlv_router_id),
265
    babel_read_router_id,
266
    NULL,
267
    NULL
268
  },
269
  [BABEL_TLV_NEXT_HOP] = {
270
    sizeof(struct babel_tlv_next_hop),
271
    babel_read_next_hop,
272
    NULL,
273
    NULL
274
  },
275
  [BABEL_TLV_UPDATE] = {
276
    sizeof(struct babel_tlv_update),
277
    babel_read_update,
278
    babel_write_update,
279
    babel_handle_update
280
  },
281
  [BABEL_TLV_ROUTE_REQUEST] = {
282
    sizeof(struct babel_tlv_route_request),
283
    babel_read_route_request,
284
    babel_write_route_request,
285
    babel_handle_route_request
286
  },
287
  [BABEL_TLV_SEQNO_REQUEST] = {
288
    sizeof(struct babel_tlv_seqno_request),
289
    babel_read_seqno_request,
290
    babel_write_seqno_request,
291
    babel_handle_seqno_request
292
  },
293
};
294

    
295
static int
296
babel_read_ack_req(struct babel_tlv *hdr, union babel_msg *m,
297
                   struct babel_parse_state *state)
298
{
299
  struct babel_tlv_ack_req *tlv = (void *) hdr;
300
  struct babel_msg_ack_req *msg = &m->ack_req;
301

    
302
  msg->type = BABEL_TLV_ACK_REQ;
303
  msg->nonce = get_u16(&tlv->nonce);
304
  msg->interval = get_time16(&tlv->interval);
305
  msg->sender = state->saddr;
306

    
307
  if (!msg->interval)
308
    return PARSE_ERROR;
309

    
310
  return PARSE_SUCCESS;
311
}
312

    
313
static uint
314
babel_write_ack(struct babel_tlv *hdr, union babel_msg *m,
315
                struct babel_write_state *state UNUSED, uint max_len UNUSED)
316
{
317
  struct babel_tlv_ack *tlv = (void *) hdr;
318
  struct babel_msg_ack *msg = &m->ack;
319

    
320
  TLV_HDR0(tlv, BABEL_TLV_ACK);
321
  put_u16(&tlv->nonce, msg->nonce);
322

    
323
  return sizeof(struct babel_tlv_ack);
324
}
325

    
326
static int
327
babel_read_hello(struct babel_tlv *hdr, union babel_msg *m,
328
                 struct babel_parse_state *state)
329
{
330
  struct babel_tlv_hello *tlv = (void *) hdr;
331
  struct babel_msg_hello *msg = &m->hello;
332

    
333
  msg->type = BABEL_TLV_HELLO;
334
  msg->seqno = get_u16(&tlv->seqno);
335
  msg->interval = get_time16(&tlv->interval);
336
  msg->sender = state->saddr;
337

    
338
  return PARSE_SUCCESS;
339
}
340

    
341
static uint
342
babel_write_hello(struct babel_tlv *hdr, union babel_msg *m,
343
                  struct babel_write_state *state UNUSED, uint max_len UNUSED)
344
{
345
  struct babel_tlv_hello *tlv = (void *) hdr;
346
  struct babel_msg_hello *msg = &m->hello;
347

    
348
  TLV_HDR0(tlv, BABEL_TLV_HELLO);
349
  put_u16(&tlv->seqno, msg->seqno);
350
  put_time16(&tlv->interval, msg->interval);
351

    
352
  return sizeof(struct babel_tlv_hello);
353
}
354

    
355
static int
356
babel_read_ihu(struct babel_tlv *hdr, union babel_msg *m,
357
               struct babel_parse_state *state)
358
{
359
  struct babel_tlv_ihu *tlv = (void *) hdr;
360
  struct babel_msg_ihu *msg = &m->ihu;
361

    
362
  msg->type = BABEL_TLV_IHU;
363
  msg->ae = tlv->ae;
364
  msg->rxcost = get_u16(&tlv->rxcost);
365
  msg->interval = get_time16(&tlv->interval);
366
  msg->addr = IPA_NONE;
367
  msg->sender = state->saddr;
368

    
369
  if (msg->ae >= BABEL_AE_MAX)
370
    return PARSE_IGNORE;
371

    
372
  // We handle link-local IPs. In every other case, the addr field will be 0 but
373
  // validation will succeed. The handler takes care of these cases.
374
  if (msg->ae == BABEL_AE_IP6_LL)
375
  {
376
    if (TLV_OPT_LENGTH(tlv) < 8)
377
      return PARSE_ERROR;
378

    
379
    msg->addr = ipa_from_ip6(get_ip6_ll(&tlv->addr));
380
  }
381

    
382
  return PARSE_SUCCESS;
383
}
384

    
385
static uint
386
babel_write_ihu(struct babel_tlv *hdr, union babel_msg *m,
387
                struct babel_write_state *state UNUSED, uint max_len)
388
{
389
  struct babel_tlv_ihu *tlv = (void *) hdr;
390
  struct babel_msg_ihu *msg = &m->ihu;
391

    
392
  if (ipa_is_link_local(msg->addr) && max_len < sizeof(struct babel_tlv_ihu) + 8)
393
    return 0;
394

    
395
  TLV_HDR0(tlv, BABEL_TLV_IHU);
396
  put_u16(&tlv->rxcost, msg->rxcost);
397
  put_time16(&tlv->interval, msg->interval);
398

    
399
  if (!ipa_is_link_local(msg->addr))
400
  {
401
    tlv->ae = BABEL_AE_WILDCARD;
402
    return sizeof(struct babel_tlv_ihu);
403
  }
404
  put_ip6_ll(&tlv->addr, msg->addr);
405
  tlv->ae = BABEL_AE_IP6_LL;
406
  hdr->length += 8;
407
  return sizeof(struct babel_tlv_ihu) + 8;
408
}
409

    
410
static int
411
babel_read_router_id(struct babel_tlv *hdr, union babel_msg *m UNUSED,
412
                     struct babel_parse_state *state)
413
{
414
  struct babel_tlv_router_id *tlv = (void *) hdr;
415

    
416
  state->router_id = get_u64(&tlv->router_id);
417
  state->router_id_seen = 1;
418

    
419
  return PARSE_IGNORE;
420
}
421

    
422
/* This is called directly from babel_write_update() */
423
static uint
424
babel_write_router_id(struct babel_tlv *hdr, u64 router_id,
425
                      struct babel_write_state *state, uint max_len UNUSED)
426
{
427
  struct babel_tlv_router_id *tlv = (void *) hdr;
428

    
429
  /* We still assume that first min_length bytes are available and zeroed */
430

    
431
  TLV_HDR0(tlv, BABEL_TLV_ROUTER_ID);
432
  put_u64(&tlv->router_id, router_id);
433

    
434
  state->router_id = router_id;
435
  state->router_id_seen = 1;
436

    
437
  return sizeof(struct babel_tlv_router_id);
438
}
439

    
440
static int
441
babel_read_next_hop(struct babel_tlv *hdr, union babel_msg *m UNUSED,
442
                    struct babel_parse_state *state)
443
{
444
  struct babel_tlv_next_hop *tlv = (void *) hdr;
445

    
446
  switch (tlv->ae)
447
  {
448
  case BABEL_AE_WILDCARD:
449
    return PARSE_ERROR;
450

    
451
  case BABEL_AE_IP4:
452
    if (TLV_OPT_LENGTH(tlv) < sizeof(ip4_addr))
453
      return PARSE_ERROR;
454

    
455
    state->next_hop_ip4 = ipa_from_ip4(get_ip4(&tlv->addr));
456
    return PARSE_IGNORE;
457

    
458
  case BABEL_AE_IP6:
459
    if (TLV_OPT_LENGTH(tlv) < sizeof(ip6_addr))
460
      return PARSE_ERROR;
461

    
462
    state->next_hop_ip6 = ipa_from_ip6(get_ip6(&tlv->addr));
463
    return PARSE_IGNORE;
464

    
465
  case BABEL_AE_IP6_LL:
466
    if (TLV_OPT_LENGTH(tlv) < 8)
467
      return PARSE_ERROR;
468

    
469
    state->next_hop_ip6 = ipa_from_ip6(get_ip6_ll(&tlv->addr));
470
    return PARSE_IGNORE;
471

    
472
  default:
473
    return PARSE_IGNORE;
474
  }
475

    
476
  return PARSE_IGNORE;
477
}
478

    
479
/* This is called directly from babel_write_update() and returns -1 if a next
480
   hop should be written but there is not enough space. */
481
static int
482
babel_write_next_hop(struct babel_tlv *hdr, ip_addr addr,
483
                     struct babel_write_state *state, uint max_len)
484
{
485
  struct babel_tlv_next_hop *tlv = (void *) hdr;
486

    
487
  if (ipa_zero(addr))
488
  {
489
    /* Should not happen */
490
    return 0;
491
  }
492
  else if (ipa_is_ip4(addr) && !ipa_equal(addr, state->next_hop_ip4))
493
  {
494
    uint len = sizeof(struct babel_tlv_next_hop) + sizeof(ip4_addr);
495
    if (len > max_len)
496
      return -1;
497

    
498
    TLV_HDR(tlv, BABEL_TLV_NEXT_HOP, len);
499

    
500
    tlv->ae = BABEL_AE_IP4;
501
    put_ip4(&tlv->addr, ipa_to_ip4(addr));
502
    state->next_hop_ip4 = addr;
503

    
504
    return len;
505
  }
506
  else if (ipa_is_ip6(addr) && !ipa_equal(addr, state->next_hop_ip6))
507
  {
508
    uint len = sizeof(struct babel_tlv_next_hop) + sizeof(ip6_addr);
509
    if (len > max_len)
510
      return -1;
511

    
512
    TLV_HDR(tlv, BABEL_TLV_NEXT_HOP, len);
513

    
514
    tlv->ae = BABEL_AE_IP6;
515
    put_ip6(&tlv->addr, ipa_to_ip6(addr));
516
    state->next_hop_ip6 = addr;
517

    
518
    return len;
519
  }
520

    
521
  return 0;
522
}
523

    
524
static int
525
babel_read_update(struct babel_tlv *hdr, union babel_msg *m,
526
                  struct babel_parse_state *state)
527
{
528
  struct babel_tlv_update *tlv = (void *) hdr;
529
  struct babel_msg_update *msg = &m->update;
530

    
531
  msg->type = BABEL_TLV_UPDATE;
532
  msg->interval = get_time16(&tlv->interval);
533
  msg->seqno = get_u16(&tlv->seqno);
534
  msg->metric = get_u16(&tlv->metric);
535

    
536
  /* Length of received prefix data without omitted part */
537
  int len = BYTES(tlv->plen) - (int) tlv->omitted;
538
  u8 buf[16] = {};
539

    
540
  if ((len < 0) || ((uint) len > TLV_OPT_LENGTH(tlv)))
541
    return PARSE_ERROR;
542

    
543
  switch (tlv->ae)
544
  {
545
  case BABEL_AE_WILDCARD:
546
    if (tlv->plen > 0)
547
      return PARSE_ERROR;
548

    
549
    if (msg->metric != 65535)
550
      return PARSE_ERROR;
551

    
552
    msg->wildcard = 1;
553
    break;
554

    
555
  case BABEL_AE_IP4:
556
    if (tlv->plen > IP4_MAX_PREFIX_LENGTH)
557
      return PARSE_ERROR;
558

    
559
    /* Cannot omit data if there is no saved prefix */
560
    if (tlv->omitted && !state->def_ip4_prefix_seen)
561
      return PARSE_ERROR;
562

    
563
    /* Need next hop for v4 routes */
564
    if (ipa_zero(state->next_hop_ip4))
565
      return PARSE_ERROR;
566

    
567
    /* Merge saved prefix and received prefix parts */
568
    memcpy(buf, state->def_ip4_prefix, tlv->omitted);
569
    memcpy(buf + tlv->omitted, tlv->addr, len);
570

    
571
    ip4_addr prefix4 = get_ip4(buf);
572
    net_fill_ip4(&msg->net, prefix4, tlv->plen);
573

    
574
    if (tlv->flags & BABEL_FLAG_DEF_PREFIX)
575
    {
576
      put_ip4(state->def_ip4_prefix, prefix4);
577
      state->def_ip4_prefix_seen = 1;
578
    }
579

    
580
    msg->next_hop = state->next_hop_ip4;
581

    
582
    break;
583

    
584
  case BABEL_AE_IP6:
585
    if (tlv->plen > IP6_MAX_PREFIX_LENGTH)
586
      return PARSE_ERROR;
587

    
588
    /* Cannot omit data if there is no saved prefix */
589
    if (tlv->omitted && !state->def_ip6_prefix_seen)
590
      return PARSE_ERROR;
591

    
592
    /* Merge saved prefix and received prefix parts */
593
    memcpy(buf, state->def_ip6_prefix, tlv->omitted);
594
    memcpy(buf + tlv->omitted, tlv->addr, len);
595

    
596
    ip6_addr prefix6 = get_ip6(buf);
597
    net_fill_ip6(&msg->net, prefix6, tlv->plen);
598

    
599
    if (tlv->flags & BABEL_FLAG_DEF_PREFIX)
600
    {
601
      put_ip6(state->def_ip6_prefix, prefix6);
602
      state->def_ip6_prefix_seen = 1;
603
    }
604

    
605
    if (tlv->flags & BABEL_FLAG_ROUTER_ID)
606
    {
607
      state->router_id = ((u64) _I2(prefix6)) << 32 | _I3(prefix6);
608
      state->router_id_seen = 1;
609
    }
610

    
611
    msg->next_hop = state->next_hop_ip6;
612

    
613
    break;
614

    
615
  case BABEL_AE_IP6_LL:
616
    /* ??? */
617
    return PARSE_IGNORE;
618

    
619
  default:
620
    return PARSE_IGNORE;
621
  }
622

    
623
  /* Update must have Router ID, unless it is retraction */
624
  if (!state->router_id_seen && (msg->metric != BABEL_INFINITY))
625
  {
626
    DBG("Babel: No router ID seen before update\n");
627
    return PARSE_ERROR;
628
  }
629

    
630
  msg->router_id = state->router_id;
631
  msg->sender = state->saddr;
632

    
633
  return PARSE_SUCCESS;
634
}
635

    
636
static uint
637
babel_write_update(struct babel_tlv *hdr, union babel_msg *m,
638
                   struct babel_write_state *state, uint max_len)
639
{
640
  struct babel_msg_update *msg = &m->update;
641
  uint len0 = 0;
642

    
643
  /*
644
   * When needed, we write Router-ID TLV before Update TLV and return size of
645
   * both of them. There is enough space for the Router-ID TLV, because
646
   * sizeof(struct babel_tlv_router_id) == sizeof(struct babel_tlv_update).
647
   *
648
   * Router ID is not used for retractions, so do not use it in such case.
649
   */
650
  if ((msg->metric < BABEL_INFINITY) &&
651
      (!state->router_id_seen || (msg->router_id != state->router_id)))
652
  {
653
    len0 = babel_write_router_id(hdr, msg->router_id, state, max_len);
654
    hdr = NEXT_TLV(hdr);
655
  }
656

    
657
  /*
658
   * We also may add Next Hop TLV for regular updates. It may fail for not
659
   * enough space or it may be unnecessary as the next hop is the same as the
660
   * last one already announced. So we handle all three cases.
661
   */
662
  if (msg->metric < BABEL_INFINITY)
663
  {
664
    int l = babel_write_next_hop(hdr, msg->next_hop, state, max_len - len0);
665
    if (l < 0)
666
      return 0;
667

    
668
    if (l)
669
    {
670
      len0 += l;
671
      hdr = NEXT_TLV(hdr);
672
    }
673
  }
674

    
675
  struct babel_tlv_update *tlv = (void *) hdr;
676
  uint len = sizeof(struct babel_tlv_update) + NET_SIZE(&msg->net);
677

    
678
  if (len0 + len > max_len)
679
    return 0;
680

    
681
  memset(tlv, 0, sizeof(struct babel_tlv_update));
682
  TLV_HDR(tlv, BABEL_TLV_UPDATE, len);
683

    
684
  if (msg->wildcard)
685
  {
686
    tlv->ae = BABEL_AE_WILDCARD;
687
    tlv->plen = 0;
688
  }
689
  else if (msg->net.type == NET_IP4)
690
  {
691
    tlv->ae = BABEL_AE_IP4;
692
    tlv->plen = net4_pxlen(&msg->net);
693
    put_ip4_px(tlv->addr, &msg->net);
694
  }
695
  else
696
  {
697
    tlv->ae = BABEL_AE_IP6;
698
    tlv->plen = net6_pxlen(&msg->net);
699
    put_ip6_px(tlv->addr, &msg->net);
700
  }
701

    
702
  put_time16(&tlv->interval, msg->interval);
703
  put_u16(&tlv->seqno, msg->seqno);
704
  put_u16(&tlv->metric, msg->metric);
705

    
706
  return len0 + len;
707
}
708

    
709
static int
710
babel_read_route_request(struct babel_tlv *hdr, union babel_msg *m,
711
                         struct babel_parse_state *state UNUSED)
712
{
713
  struct babel_tlv_route_request *tlv = (void *) hdr;
714
  struct babel_msg_route_request *msg = &m->route_request;
715

    
716
  msg->type = BABEL_TLV_ROUTE_REQUEST;
717

    
718
  switch (tlv->ae)
719
  {
720
  case BABEL_AE_WILDCARD:
721
    /* Wildcard requests must have plen 0 */
722
    if (tlv->plen > 0)
723
      return PARSE_ERROR;
724

    
725
    msg->full = 1;
726
    return PARSE_SUCCESS;
727

    
728
  case BABEL_AE_IP4:
729
    if (tlv->plen > IP4_MAX_PREFIX_LENGTH)
730
      return PARSE_ERROR;
731

    
732
    if (TLV_OPT_LENGTH(tlv) < BYTES(tlv->plen))
733
      return PARSE_ERROR;
734

    
735
    read_ip4_px(&msg->net, tlv->addr, tlv->plen);
736
    return PARSE_SUCCESS;
737

    
738
  case BABEL_AE_IP6:
739
    if (tlv->plen > IP6_MAX_PREFIX_LENGTH)
740
      return PARSE_ERROR;
741

    
742
    if (TLV_OPT_LENGTH(tlv) < BYTES(tlv->plen))
743
      return PARSE_ERROR;
744

    
745
    read_ip6_px(&msg->net, tlv->addr, tlv->plen);
746
    return PARSE_SUCCESS;
747

    
748
  case BABEL_AE_IP6_LL:
749
    return PARSE_ERROR;
750

    
751
  default:
752
    return PARSE_IGNORE;
753
  }
754

    
755
  return PARSE_IGNORE;
756
}
757

    
758
static uint
759
babel_write_route_request(struct babel_tlv *hdr, union babel_msg *m,
760
                          struct babel_write_state *state UNUSED, uint max_len)
761
{
762
  struct babel_tlv_route_request *tlv = (void *) hdr;
763
  struct babel_msg_route_request *msg = &m->route_request;
764

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

    
767
  if (len > max_len)
768
    return 0;
769

    
770
  TLV_HDR(tlv, BABEL_TLV_ROUTE_REQUEST, len);
771

    
772
  if (msg->full)
773
  {
774
    tlv->ae = BABEL_AE_WILDCARD;
775
    tlv->plen = 0;
776
  }
777
  else if (msg->net.type == NET_IP4)
778
  {
779
    tlv->ae = BABEL_AE_IP4;
780
    tlv->plen = net4_pxlen(&msg->net);
781
    put_ip4_px(tlv->addr, &msg->net);
782
  }
783
  else
784
  {
785
    tlv->ae = BABEL_AE_IP6;
786
    tlv->plen = net6_pxlen(&msg->net);
787
    put_ip6_px(tlv->addr, &msg->net);
788
  }
789

    
790
  return len;
791
}
792

    
793
static int
794
babel_read_seqno_request(struct babel_tlv *hdr, union babel_msg *m,
795
                         struct babel_parse_state *state)
796
{
797
  struct babel_tlv_seqno_request *tlv = (void *) hdr;
798
  struct babel_msg_seqno_request *msg = &m->seqno_request;
799

    
800
  msg->type = BABEL_TLV_SEQNO_REQUEST;
801
  msg->seqno = get_u16(&tlv->seqno);
802
  msg->hop_count = tlv->hop_count;
803
  msg->router_id = get_u64(&tlv->router_id);
804
  msg->sender = state->saddr;
805

    
806
  if (tlv->hop_count == 0)
807
    return PARSE_ERROR;
808

    
809
  switch (tlv->ae)
810
  {
811
  case BABEL_AE_WILDCARD:
812
    return PARSE_ERROR;
813

    
814
  case BABEL_AE_IP4:
815
    if (tlv->plen > IP4_MAX_PREFIX_LENGTH)
816
      return PARSE_ERROR;
817

    
818
    if (TLV_OPT_LENGTH(tlv) < BYTES(tlv->plen))
819
      return PARSE_ERROR;
820

    
821
    read_ip4_px(&msg->net, tlv->addr, tlv->plen);
822
    return PARSE_SUCCESS;
823

    
824
  case BABEL_AE_IP6:
825
    if (tlv->plen > IP6_MAX_PREFIX_LENGTH)
826
      return PARSE_ERROR;
827

    
828
    if (TLV_OPT_LENGTH(tlv) < BYTES(tlv->plen))
829
      return PARSE_ERROR;
830

    
831
    read_ip6_px(&msg->net, tlv->addr, tlv->plen);
832
    return PARSE_SUCCESS;
833

    
834
  case BABEL_AE_IP6_LL:
835
    return PARSE_ERROR;
836

    
837
  default:
838
    return PARSE_IGNORE;
839
  }
840

    
841
  return PARSE_IGNORE;
842
}
843

    
844
static uint
845
babel_write_seqno_request(struct babel_tlv *hdr, union babel_msg *m,
846
                          struct babel_write_state *state UNUSED, uint max_len)
847
{
848
  struct babel_tlv_seqno_request *tlv = (void *) hdr;
849
  struct babel_msg_seqno_request *msg = &m->seqno_request;
850

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

    
853
  if (len > max_len)
854
    return 0;
855

    
856
  TLV_HDR(tlv, BABEL_TLV_SEQNO_REQUEST, len);
857

    
858
  if (msg->net.type == NET_IP4)
859
  {
860
    tlv->ae = BABEL_AE_IP4;
861
    tlv->plen = net4_pxlen(&msg->net);
862
    put_ip4_px(tlv->addr, &msg->net);
863
  }
864
  else
865
  {
866
    tlv->ae = BABEL_AE_IP6;
867
    tlv->plen = net6_pxlen(&msg->net);
868
    put_ip6_px(tlv->addr, &msg->net);
869
  }
870

    
871
  put_u16(&tlv->seqno, msg->seqno);
872
  tlv->hop_count = msg->hop_count;
873
  put_u64(&tlv->router_id, msg->router_id);
874

    
875
  return len;
876
}
877

    
878
static inline int
879
babel_read_tlv(struct babel_tlv *hdr,
880
               union babel_msg *msg,
881
               struct babel_parse_state *state)
882
{
883
  if ((hdr->type <= BABEL_TLV_PADN) ||
884
      (hdr->type >= BABEL_TLV_MAX) ||
885
      !tlv_data[hdr->type].read_tlv)
886
    return PARSE_IGNORE;
887

    
888
  if (TLV_LENGTH(hdr) < tlv_data[hdr->type].min_length)
889
    return PARSE_ERROR;
890

    
891
  memset(msg, 0, sizeof(*msg));
892
  return tlv_data[hdr->type].read_tlv(hdr, msg, state);
893
}
894

    
895
static uint
896
babel_write_tlv(struct babel_tlv *hdr,
897
                union babel_msg *msg,
898
                struct babel_write_state *state,
899
                uint max_len)
900
{
901
  if ((msg->type <= BABEL_TLV_PADN) ||
902
      (msg->type >= BABEL_TLV_MAX) ||
903
      !tlv_data[msg->type].write_tlv)
904
    return 0;
905

    
906
  if (tlv_data[msg->type].min_length > max_len)
907
    return 0;
908

    
909
  memset(hdr, 0, tlv_data[msg->type].min_length);
910
  return tlv_data[msg->type].write_tlv(hdr, msg, state, max_len);
911
}
912

    
913

    
914
/*
915
 *        Packet RX/TX functions
916
 */
917

    
918
static int
919
babel_send_to(struct babel_iface *ifa, ip_addr dest)
920
{
921
  sock *sk = ifa->sk;
922
  struct babel_pkt_header *hdr = (void *) sk->tbuf;
923
  int len = get_u16(&hdr->length) + sizeof(struct babel_pkt_header);
924

    
925
  DBG("Babel: Sending %d bytes to %I\n", len, dest);
926
  return sk_send_to(sk, len, dest, 0);
927
}
928

    
929
/**
930
 * babel_write_queue - Write a TLV queue to a transmission buffer
931
 * @ifa: Interface holding the transmission buffer
932
 * @queue: TLV queue to write (containing internal-format TLVs)
933
 *
934
 * This function writes a packet to the interface transmission buffer with as
935
 * many TLVs from the &queue as will fit in the buffer. It returns the number of
936
 * bytes written (NOT counting the packet header). The function is called by
937
 * babel_send_queue() and babel_send_unicast() to construct packets for
938
 * transmission, and uses per-TLV helper functions to convert the
939
 * internal-format TLVs to their wire representations.
940
 *
941
 * The TLVs in the queue are freed after they are written to the buffer.
942
 */
943
static uint
944
babel_write_queue(struct babel_iface *ifa, list *queue)
945
{
946
  struct babel_proto *p = ifa->proto;
947
  struct babel_write_state state = { .next_hop_ip6 = ifa->addr };
948

    
949
  if (EMPTY_LIST(*queue))
950
    return 0;
951

    
952
  byte *pos = ifa->sk->tbuf;
953
  byte *end = pos + ifa->tx_length;
954

    
955
  struct babel_pkt_header *pkt = (void *) pos;
956
  pkt->magic = BABEL_MAGIC;
957
  pkt->version = BABEL_VERSION;
958
  pkt->length = 0;
959
  pos += sizeof(struct babel_pkt_header);
960

    
961
  struct babel_msg_node *msg;
962
  WALK_LIST_FIRST(msg, *queue)
963
  {
964
    if (pos >= end)
965
      break;
966

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

    
969
    if (!len)
970
      break;
971

    
972
    pos += len;
973
    rem_node(NODE msg);
974
    sl_free(p->msg_slab, msg);
975
  }
976

    
977
  uint plen = pos - (byte *) pkt;
978
  put_u16(&pkt->length, plen - sizeof(struct babel_pkt_header));
979

    
980
  return plen;
981
}
982

    
983
void
984
babel_send_queue(void *arg)
985
{
986
  struct babel_iface *ifa = arg;
987
  while ((babel_write_queue(ifa, &ifa->msg_queue) > 0) &&
988
         (babel_send_to(ifa, IP6_BABEL_ROUTERS) > 0));
989
}
990

    
991
static inline void
992
babel_kick_queue(struct babel_iface *ifa)
993
{
994
  /*
995
   * Only schedule send event if there is not already data in the socket buffer.
996
   * Otherwise we may overwrite the data already in the buffer.
997
   */
998

    
999
  if ((ifa->sk->tpos == ifa->sk->tbuf) && !ev_active(ifa->send_event))
1000
    ev_schedule(ifa->send_event);
1001
}
1002

    
1003
/**
1004
 * babel_send_unicast - send a single TLV via unicast to a destination
1005
 * @msg: TLV to send
1006
 * @ifa: Interface to send via
1007
 * @dest: Destination of the TLV
1008
 *
1009
 * This function is used to send a single TLV via unicast to a designated
1010
 * receiver. This is used for replying to certain incoming requests, and for
1011
 * sending unicast requests to refresh routes before they expire.
1012
 */
1013
void
1014
babel_send_unicast(union babel_msg *msg, struct babel_iface *ifa, ip_addr dest)
1015
{
1016
  struct babel_proto *p = ifa->proto;
1017
  struct babel_msg_node *msgn = sl_alloc(p->msg_slab);
1018
  list queue;
1019

    
1020
  msgn->msg = *msg;
1021
  init_list(&queue);
1022
  add_tail(&queue, NODE msgn);
1023
  babel_write_queue(ifa, &queue);
1024
  babel_send_to(ifa, dest);
1025

    
1026
  /* We could overwrite waiting packet here, we may have to kick TX queue */
1027
  if (!EMPTY_LIST(ifa->msg_queue))
1028
    babel_kick_queue(ifa);
1029
}
1030

    
1031
/**
1032
 * babel_enqueue - enqueue a TLV for transmission on an interface
1033
 * @msg: TLV to enqueue (in internal TLV format)
1034
 * @ifa: Interface to enqueue to
1035
 *
1036
 * This function is called to enqueue a TLV for subsequent transmission on an
1037
 * interface. The transmission event is triggered whenever a TLV is enqueued;
1038
 * this ensures that TLVs will be transmitted in a timely manner, but that TLVs
1039
 * which are enqueued in rapid succession can be transmitted together in one
1040
 * packet.
1041
 */
1042
void
1043
babel_enqueue(union babel_msg *msg, struct babel_iface *ifa)
1044
{
1045
  struct babel_proto *p = ifa->proto;
1046
  struct babel_msg_node *msgn = sl_alloc(p->msg_slab);
1047
  msgn->msg = *msg;
1048
  add_tail(&ifa->msg_queue, NODE msgn);
1049
  babel_kick_queue(ifa);
1050
}
1051

    
1052
/**
1053
 * babel_process_packet - process incoming data packet
1054
 * @pkt: Pointer to the packet data
1055
 * @len: Length of received packet
1056
 * @saddr: Address of packet sender
1057
 * @ifa: Interface packet was received on.
1058
 *
1059
 * This function is the main processing hook of incoming Babel packets. It
1060
 * checks that the packet header is well-formed, then processes the TLVs
1061
 * contained in the packet. This is done in two passes: First all TLVs are
1062
 * parsed into the internal TLV format. If a TLV parser fails, processing of the
1063
 * rest of the packet is aborted.
1064
 *
1065
 * After the parsing step, the TLV handlers are called for each parsed TLV in
1066
 * order.
1067
 */
1068
static void
1069
babel_process_packet(struct babel_pkt_header *pkt, int len,
1070
                     ip_addr saddr, struct babel_iface *ifa)
1071
{
1072
  struct babel_proto *p = ifa->proto;
1073
  struct babel_tlv *tlv;
1074
  struct babel_msg_node *msg;
1075
  list msgs;
1076
  int res;
1077

    
1078
  int plen = sizeof(struct babel_pkt_header) + get_u16(&pkt->length);
1079
  byte *pos;
1080
  byte *end = (byte *)pkt + plen;
1081

    
1082
  struct babel_parse_state state = {
1083
    .proto          = p,
1084
    .ifa          = ifa,
1085
    .saddr          = saddr,
1086
    .next_hop_ip6 = saddr,
1087
  };
1088

    
1089
  if ((pkt->magic != BABEL_MAGIC) || (pkt->version != BABEL_VERSION))
1090
  {
1091
    TRACE(D_PACKETS, "Strange packet from %I via %s - magic %d version %d",
1092
          saddr, ifa->iface->name, pkt->magic, pkt->version);
1093
    return;
1094
  }
1095

    
1096
  if (plen > len)
1097
  {
1098
    LOG_PKT("Bad packet from %I via %s - %s (%u)",
1099
            saddr, ifa->iface->name, "length mismatch", plen);
1100
    return;
1101
  }
1102

    
1103
  TRACE(D_PACKETS, "Packet received from %I via %s",
1104
        saddr, ifa->iface->name);
1105

    
1106
  init_list(&msgs);
1107

    
1108
  /* First pass through the packet TLV by TLV, parsing each into internal data
1109
     structures. */
1110
  for (tlv = FIRST_TLV(pkt);
1111
       (byte *)tlv < end;
1112
       tlv = NEXT_TLV(tlv))
1113
  {
1114
    /* Ugly special case */
1115
    if (tlv->type == BABEL_TLV_PAD1)
1116
      continue;
1117

    
1118
    /* The end of the common TLV header */
1119
    pos = (byte *)tlv + sizeof(struct babel_tlv);
1120
    if ((pos > end) || (pos + tlv->length > end))
1121
    {
1122
      LOG_PKT("Bad TLV from %I via %s type %d pos %d - framing error",
1123
              saddr, ifa->iface->name, tlv->type, (byte *)tlv - (byte *)pkt);
1124
      break;
1125
    }
1126

    
1127
    msg = sl_alloc(p->msg_slab);
1128
    res = babel_read_tlv(tlv, &msg->msg, &state);
1129
    if (res == PARSE_SUCCESS)
1130
    {
1131
      add_tail(&msgs, NODE msg);
1132
    }
1133
    else if (res == PARSE_IGNORE)
1134
    {
1135
      DBG("Babel: Ignoring TLV of type %d\n", tlv->type);
1136
      sl_free(p->msg_slab, msg);
1137
    }
1138
    else /* PARSE_ERROR */
1139
    {
1140
      LOG_PKT("Bad TLV from %I via %s type %d pos %d - parse error",
1141
              saddr, ifa->iface->name, tlv->type, (byte *)tlv - (byte *)pkt);
1142
      sl_free(p->msg_slab, msg);
1143
      break;
1144
    }
1145
  }
1146

    
1147
  /* Parsing done, handle all parsed TLVs */
1148
  WALK_LIST_FIRST(msg, msgs)
1149
  {
1150
    if (tlv_data[msg->msg.type].handle_tlv)
1151
      tlv_data[msg->msg.type].handle_tlv(&msg->msg, ifa);
1152
    rem_node(NODE msg);
1153
    sl_free(p->msg_slab, msg);
1154
  }
1155
}
1156

    
1157
static void
1158
babel_err_hook(sock *sk, int err)
1159
{
1160
  struct babel_iface *ifa = sk->data;
1161
  struct babel_proto *p = ifa->proto;
1162

    
1163
  log(L_ERR "%s: Socket error on %s: %M", p->p.name, ifa->iface->name, err);
1164
  /* FIXME: Drop queued TLVs here? */
1165
}
1166

    
1167

    
1168
static void
1169
babel_tx_hook(sock *sk)
1170
{
1171
  struct babel_iface *ifa = sk->data;
1172

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

    
1176
  babel_send_queue(ifa);
1177
}
1178

    
1179

    
1180
static int
1181
babel_rx_hook(sock *sk, uint len)
1182
{
1183
  struct babel_iface *ifa = sk->data;
1184
  struct babel_proto *p = ifa->proto;
1185
  const char *err_dsc = NULL;
1186
  uint err_val = 0;
1187

    
1188
  if (sk->lifindex != ifa->iface->index)
1189
    return 1;
1190

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

    
1194
  /* Silently ignore my own packets */
1195
  if (ipa_equal(ifa->iface->addr->ip, sk->faddr))
1196
    return 1;
1197

    
1198
  if (!ipa_is_link_local(sk->faddr))
1199
    DROP1("wrong src address");
1200

    
1201
  if (sk->fport != ifa->cf->port)
1202
    DROP("wrong src port", sk->fport);
1203

    
1204
  if (len < sizeof(struct babel_pkt_header))
1205
    DROP("too short", len);
1206

    
1207
  if (sk->flags & SKF_TRUNCATED)
1208
    DROP("truncated", len);
1209

    
1210
  babel_process_packet((struct babel_pkt_header *) sk->rbuf, len, sk->faddr, ifa);
1211
  return 1;
1212

    
1213
drop:
1214
  LOG_PKT("Bad packet from %I via %s - %s (%u)",
1215
          sk->faddr, sk->iface->name, err_dsc, err_val);
1216
  return 1;
1217
}
1218

    
1219
int
1220
babel_open_socket(struct babel_iface *ifa)
1221
{
1222
  struct babel_proto *p = ifa->proto;
1223

    
1224
  sock *sk;
1225
  sk = sk_new(ifa->pool);
1226
  sk->type = SK_UDP;
1227
  sk->sport = ifa->cf->port;
1228
  sk->dport = ifa->cf->port;
1229
  sk->iface = ifa->iface;
1230

    
1231
  sk->rx_hook = babel_rx_hook;
1232
  sk->tx_hook = babel_tx_hook;
1233
  sk->err_hook = babel_err_hook;
1234
  sk->data = ifa;
1235

    
1236
  sk->tos = ifa->cf->tx_tos;
1237
  sk->priority = ifa->cf->tx_priority;
1238
  sk->ttl = 1;
1239
  sk->flags = SKF_LADDR_RX;
1240

    
1241
  if (sk_open(sk) < 0)
1242
    goto err;
1243

    
1244
  if (sk_setup_multicast(sk) < 0)
1245
    goto err;
1246

    
1247
  if (sk_join_group(sk, IP6_BABEL_ROUTERS) < 0)
1248
    goto err;
1249

    
1250
  ifa->sk = sk;
1251
  return 1;
1252

    
1253
err:
1254
  sk_log_error(sk, p->p.name);
1255
  rfree(sk);
1256
  return 0;
1257
}