Statistics
| Branch: | Revision:

iof-bird-daemon / proto / babel / packets.c @ 5e8df049

History | View | Annotate | Download (27.3 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;
116
  u64 router_id;                /* Router ID used in subsequent updates */
117
  u8 def_ip6_prefix[16];        /* Implicit IPv6 prefix in network order */
118
  u8 def_ip4_prefix[4];                /* Implicit IPv4 prefix in network order */
119
  u8 router_id_seen;                /* router_id field is valid */
120
  u8 def_ip6_prefix_seen;        /* def_ip6_prefix is valid */
121
  u8 def_ip4_prefix_seen;        /* def_ip4_prefix is valid */
122
};
123

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

    
130
struct babel_write_state {
131
  u64 router_id;
132
  u8 router_id_seen;
133
//  ip_addr next_hop;
134
};
135

    
136

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

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

    
149
#define NET_SIZE(n) BYTES(net_pxlen(n))
150

    
151

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

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

    
165
static inline void
166
read_ip6_px(net_addr *n, const void *p, uint plen)
167
{
168
  ip6_addr addr = IPA_NONE;
169
  memcpy(&addr, p, BYTES(plen));
170
  net_fill_ip6(n, ip6_ntoh(addr), plen);
171
}
172

    
173
static inline void
174
put_ip6_px(void *p, net_addr *n)
175
{
176
  ip6_addr addr = ip6_hton(net6_prefix(n));
177
  memcpy(p, &addr, NET_SIZE(n));
178
}
179

    
180
static inline ip6_addr
181
get_ip6_ll(const void *p)
182
{
183
  return ip6_build(0xfe800000, 0, get_u32(p+0), get_u32(p+4));
184
}
185

    
186
static inline void
187
put_ip6_ll(void *p, ip6_addr addr)
188
{
189
  put_u32(p+0, _I2(addr));
190
  put_u32(p+4, _I3(addr));
191
}
192

    
193

    
194
/*
195
 *        TLV read/write functions
196
 */
197

    
198
static int babel_read_ack_req(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
199
static int babel_read_hello(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
200
static int babel_read_ihu(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
201
static int babel_read_router_id(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
202
static int babel_read_next_hop(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
203
static int babel_read_update(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
204
static int babel_read_route_request(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
205
static int babel_read_seqno_request(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
206

    
207
static uint babel_write_ack(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, uint max_len);
208
static uint babel_write_hello(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, uint max_len);
209
static uint babel_write_ihu(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, uint max_len);
210
static uint babel_write_update(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, uint max_len);
211
static uint babel_write_route_request(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, uint max_len);
212
static uint babel_write_seqno_request(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, uint max_len);
213

    
214
struct babel_tlv_data {
215
  u8 min_length;
216
  int (*read_tlv)(struct babel_tlv *hdr, union babel_msg *m, struct babel_parse_state *state);
217
  uint (*write_tlv)(struct babel_tlv *hdr, union babel_msg *m, struct babel_write_state *state, uint max_len);
218
  void (*handle_tlv)(union babel_msg *m, struct babel_iface *ifa);
219
};
220

    
221
static const struct babel_tlv_data tlv_data[BABEL_TLV_MAX] = {
222
  [BABEL_TLV_ACK_REQ] = {
223
    sizeof(struct babel_tlv_ack_req),
224
    babel_read_ack_req,
225
    NULL,
226
    babel_handle_ack_req
227
  },
228
  [BABEL_TLV_ACK] = {
229
    sizeof(struct babel_tlv_ack),
230
    NULL,
231
    babel_write_ack,
232
    NULL
233
  },
234
  [BABEL_TLV_HELLO] = {
235
    sizeof(struct babel_tlv_hello),
236
    babel_read_hello,
237
    babel_write_hello,
238
    babel_handle_hello
239
  },
240
  [BABEL_TLV_IHU] = {
241
    sizeof(struct babel_tlv_ihu),
242
    babel_read_ihu,
243
    babel_write_ihu,
244
    babel_handle_ihu
245
  },
246
  [BABEL_TLV_ROUTER_ID] = {
247
    sizeof(struct babel_tlv_router_id),
248
    babel_read_router_id,
249
    NULL,
250
    NULL
251
  },
252
  [BABEL_TLV_NEXT_HOP] = {
253
    sizeof(struct babel_tlv_next_hop),
254
    babel_read_next_hop,
255
    NULL,
256
    NULL
257
  },
258
  [BABEL_TLV_UPDATE] = {
259
    sizeof(struct babel_tlv_update),
260
    babel_read_update,
261
    babel_write_update,
262
    babel_handle_update
263
  },
264
  [BABEL_TLV_ROUTE_REQUEST] = {
265
    sizeof(struct babel_tlv_route_request),
266
    babel_read_route_request,
267
    babel_write_route_request,
268
    babel_handle_route_request
269
  },
270
  [BABEL_TLV_SEQNO_REQUEST] = {
271
    sizeof(struct babel_tlv_seqno_request),
272
    babel_read_seqno_request,
273
    babel_write_seqno_request,
274
    babel_handle_seqno_request
275
  },
276
};
277

    
278
static int
279
babel_read_ack_req(struct babel_tlv *hdr, union babel_msg *m,
280
                   struct babel_parse_state *state)
281
{
282
  struct babel_tlv_ack_req *tlv = (void *) hdr;
283
  struct babel_msg_ack_req *msg = &m->ack_req;
284

    
285
  msg->type = BABEL_TLV_ACK_REQ;
286
  msg->nonce = get_u16(&tlv->nonce);
287
  msg->interval = get_time16(&tlv->interval);
288
  msg->sender = state->saddr;
289

    
290
  if (!msg->interval)
291
    return PARSE_ERROR;
292

    
293
  return PARSE_SUCCESS;
294
}
295

    
296
static uint
297
babel_write_ack(struct babel_tlv *hdr, union babel_msg *m,
298
                struct babel_write_state *state UNUSED, uint max_len UNUSED)
299
{
300
  struct babel_tlv_ack *tlv = (void *) hdr;
301
  struct babel_msg_ack *msg = &m->ack;
302

    
303
  TLV_HDR0(tlv, BABEL_TLV_ACK);
304
  put_u16(&tlv->nonce, msg->nonce);
305

    
306
  return sizeof(struct babel_tlv_ack);
307
}
308

    
309
static int
310
babel_read_hello(struct babel_tlv *hdr, union babel_msg *m,
311
                 struct babel_parse_state *state)
312
{
313
  struct babel_tlv_hello *tlv = (void *) hdr;
314
  struct babel_msg_hello *msg = &m->hello;
315

    
316
  msg->type = BABEL_TLV_HELLO;
317
  msg->seqno = get_u16(&tlv->seqno);
318
  msg->interval = get_time16(&tlv->interval);
319
  msg->sender = state->saddr;
320

    
321
  return PARSE_SUCCESS;
322
}
323

    
324
static uint
325
babel_write_hello(struct babel_tlv *hdr, union babel_msg *m,
326
                  struct babel_write_state *state UNUSED, uint max_len UNUSED)
327
{
328
  struct babel_tlv_hello *tlv = (void *) hdr;
329
  struct babel_msg_hello *msg = &m->hello;
330

    
331
  TLV_HDR0(tlv, BABEL_TLV_HELLO);
332
  put_u16(&tlv->seqno, msg->seqno);
333
  put_time16(&tlv->interval, msg->interval);
334

    
335
  return sizeof(struct babel_tlv_hello);
336
}
337

    
338
static int
339
babel_read_ihu(struct babel_tlv *hdr, union babel_msg *m,
340
               struct babel_parse_state *state)
341
{
342
  struct babel_tlv_ihu *tlv = (void *) hdr;
343
  struct babel_msg_ihu *msg = &m->ihu;
344

    
345
  msg->type = BABEL_TLV_IHU;
346
  msg->ae = tlv->ae;
347
  msg->rxcost = get_u16(&tlv->rxcost);
348
  msg->interval = get_time16(&tlv->interval);
349
  msg->addr = IPA_NONE;
350
  msg->sender = state->saddr;
351

    
352
  if (msg->ae >= BABEL_AE_MAX)
353
    return PARSE_IGNORE;
354

    
355
  // We handle link-local IPs. In every other case, the addr field will be 0 but
356
  // validation will succeed. The handler takes care of these cases.
357
  if (msg->ae == BABEL_AE_IP6_LL)
358
  {
359
    if (TLV_OPT_LENGTH(tlv) < 8)
360
      return PARSE_ERROR;
361

    
362
    msg->addr = ipa_from_ip6(get_ip6_ll(&tlv->addr));
363
  }
364

    
365
  return PARSE_SUCCESS;
366
}
367

    
368
static uint
369
babel_write_ihu(struct babel_tlv *hdr, union babel_msg *m,
370
                struct babel_write_state *state UNUSED, uint max_len)
371
{
372
  struct babel_tlv_ihu *tlv = (void *) hdr;
373
  struct babel_msg_ihu *msg = &m->ihu;
374

    
375
  if (ipa_is_link_local(msg->addr) && max_len < sizeof(struct babel_tlv_ihu) + 8)
376
    return 0;
377

    
378
  TLV_HDR0(tlv, BABEL_TLV_IHU);
379
  put_u16(&tlv->rxcost, msg->rxcost);
380
  put_time16(&tlv->interval, msg->interval);
381

    
382
  if (!ipa_is_link_local(msg->addr))
383
  {
384
    tlv->ae = BABEL_AE_WILDCARD;
385
    return sizeof(struct babel_tlv_ihu);
386
  }
387
  put_ip6_ll(&tlv->addr, msg->addr);
388
  tlv->ae = BABEL_AE_IP6_LL;
389
  hdr->length += 8;
390
  return sizeof(struct babel_tlv_ihu) + 8;
391
}
392

    
393
static int
394
babel_read_router_id(struct babel_tlv *hdr, union babel_msg *m UNUSED,
395
                     struct babel_parse_state *state)
396
{
397
  struct babel_tlv_router_id *tlv = (void *) hdr;
398

    
399
  state->router_id = get_u64(&tlv->router_id);
400
  state->router_id_seen = 1;
401

    
402
  return PARSE_IGNORE;
403
}
404

    
405
/* This is called directly from babel_write_update() */
406
static uint
407
babel_write_router_id(struct babel_tlv *hdr, u64 router_id,
408
                      struct babel_write_state *state, uint max_len UNUSED)
409
{
410
  struct babel_tlv_router_id *tlv = (void *) hdr;
411

    
412
  /* We still assume that first min_length bytes are available and zeroed */
413

    
414
  TLV_HDR0(tlv, BABEL_TLV_ROUTER_ID);
415
  put_u64(&tlv->router_id, router_id);
416

    
417
  state->router_id = router_id;
418
  state->router_id_seen = 1;
419

    
420
  return sizeof(struct babel_tlv_router_id);
421
}
422

    
423
static int
424
babel_read_next_hop(struct babel_tlv *hdr, union babel_msg *m UNUSED,
425
                    struct babel_parse_state *state)
426
{
427
  struct babel_tlv_next_hop *tlv = (void *) hdr;
428

    
429
  switch (tlv->ae)
430
  {
431
  case BABEL_AE_WILDCARD:
432
    return PARSE_ERROR;
433

    
434
  case BABEL_AE_IP4:
435
    /* TODO */
436
    return PARSE_IGNORE;
437

    
438
  case BABEL_AE_IP6:
439
    if (TLV_OPT_LENGTH(tlv) < sizeof(ip6_addr))
440
      return PARSE_ERROR;
441

    
442
    state->next_hop = ipa_from_ip6(get_ip6(&tlv->addr));
443
    return PARSE_IGNORE;
444

    
445
  case BABEL_AE_IP6_LL:
446
    if (TLV_OPT_LENGTH(tlv) < 8)
447
      return PARSE_ERROR;
448

    
449
    state->next_hop = ipa_from_ip6(get_ip6_ll(&tlv->addr));
450
    return PARSE_IGNORE;
451

    
452
  default:
453
    return PARSE_IGNORE;
454
  }
455

    
456
  return PARSE_IGNORE;
457
}
458

    
459
static int
460
babel_read_update(struct babel_tlv *hdr, union babel_msg *m,
461
                  struct babel_parse_state *state)
462
{
463
  struct babel_tlv_update *tlv = (void *) hdr;
464
  struct babel_msg_update *msg = &m->update;
465

    
466
  msg->type = BABEL_TLV_UPDATE;
467
  msg->interval = get_time16(&tlv->interval);
468
  msg->seqno = get_u16(&tlv->seqno);
469
  msg->metric = get_u16(&tlv->metric);
470

    
471
  /* Length of received prefix data without omitted part */
472
  int len = BYTES(tlv->plen) - (int) tlv->omitted;
473
  u8 buf[16] = {};
474

    
475
  if ((len < 0) || ((uint) len > TLV_OPT_LENGTH(tlv)))
476
    return PARSE_ERROR;
477

    
478
  switch (tlv->ae)
479
  {
480
  case BABEL_AE_WILDCARD:
481
    if (tlv->plen > 0)
482
      return PARSE_ERROR;
483

    
484
    if (msg->metric != 65535)
485
      return PARSE_ERROR;
486

    
487
    msg->wildcard = 1;
488
    break;
489

    
490
  case BABEL_AE_IP4:
491
    /* TODO */
492
    return PARSE_IGNORE;
493

    
494
  case BABEL_AE_IP6:
495
    if (tlv->plen > IP6_MAX_PREFIX_LENGTH)
496
      return PARSE_ERROR;
497

    
498
    /* Cannot omit data if there is no saved prefix */
499
    if (tlv->omitted && !state->def_ip6_prefix_seen)
500
      return PARSE_ERROR;
501

    
502
    /* Merge saved prefix and received prefix parts */
503
    memcpy(buf, state->def_ip6_prefix, tlv->omitted);
504
    memcpy(buf + tlv->omitted, tlv->addr, len);
505

    
506
    ip6_addr prefix = get_ip6(buf);
507
    net_fill_ip6(&msg->net, prefix, tlv->plen);
508

    
509
    if (tlv->flags & BABEL_FLAG_DEF_PREFIX)
510
    {
511
      put_ip6(state->def_ip6_prefix, prefix);
512
      state->def_ip6_prefix_seen = 1;
513
    }
514

    
515
    if (tlv->flags & BABEL_FLAG_ROUTER_ID)
516
    {
517
      state->router_id = ((u64) _I2(prefix)) << 32 | _I3(prefix);
518
      state->router_id_seen = 1;
519
    }
520
    break;
521

    
522
  case BABEL_AE_IP6_LL:
523
    /* ??? */
524
    return PARSE_IGNORE;
525

    
526
  default:
527
    return PARSE_IGNORE;
528
  }
529

    
530
  /* Update must have Router ID, unless it is retraction */
531
  if (!state->router_id_seen && (msg->metric != BABEL_INFINITY))
532
  {
533
    DBG("Babel: No router ID seen before update\n");
534
    return PARSE_ERROR;
535
  }
536

    
537
  msg->router_id = state->router_id;
538
  msg->next_hop = state->next_hop;
539
  msg->sender = state->saddr;
540

    
541
  return PARSE_SUCCESS;
542
}
543

    
544
static uint
545
babel_write_update(struct babel_tlv *hdr, union babel_msg *m,
546
                   struct babel_write_state *state, uint max_len)
547
{
548
  struct babel_tlv_update *tlv = (void *) hdr;
549
  struct babel_msg_update *msg = &m->update;
550
  uint len0 = 0;
551

    
552
  /*
553
   * When needed, we write Router-ID TLV before Update TLV and return size of
554
   * both of them. There is enough space for the Router-ID TLV, because
555
   * sizeof(struct babel_tlv_router_id) == sizeof(struct babel_tlv_update).
556
   *
557
   * Router ID is not used for retractions, so do not us it in such case.
558
   */
559
  if ((msg->metric < BABEL_INFINITY) &&
560
      (!state->router_id_seen || (msg->router_id != state->router_id)))
561
  {
562
    len0 = babel_write_router_id(hdr, msg->router_id, state, max_len);
563
    tlv = (struct babel_tlv_update *) NEXT_TLV(tlv);
564
  }
565

    
566
  uint len = sizeof(struct babel_tlv_update) + NET_SIZE(&msg->net);
567

    
568
  if (len0 + len > max_len)
569
    return 0;
570

    
571
  memset(tlv, 0, sizeof(struct babel_tlv_update));
572
  TLV_HDR(tlv, BABEL_TLV_UPDATE, len);
573

    
574
  if (msg->wildcard)
575
  {
576
    tlv->ae = BABEL_AE_WILDCARD;
577
    tlv->plen = 0;
578
  }
579
  else
580
  {
581
    tlv->ae = BABEL_AE_IP6;
582
    tlv->plen = net6_pxlen(&msg->net);
583
    put_ip6_px(tlv->addr, &msg->net);
584
  }
585

    
586
  put_time16(&tlv->interval, msg->interval);
587
  put_u16(&tlv->seqno, msg->seqno);
588
  put_u16(&tlv->metric, msg->metric);
589

    
590
  return len0 + len;
591
}
592

    
593
static int
594
babel_read_route_request(struct babel_tlv *hdr, union babel_msg *m,
595
                         struct babel_parse_state *state UNUSED)
596
{
597
  struct babel_tlv_route_request *tlv = (void *) hdr;
598
  struct babel_msg_route_request *msg = &m->route_request;
599

    
600
  msg->type = BABEL_TLV_ROUTE_REQUEST;
601

    
602
  switch (tlv->ae)
603
  {
604
  case BABEL_AE_WILDCARD:
605
    /* Wildcard requests must have plen 0 */
606
    if (tlv->plen > 0)
607
      return PARSE_ERROR;
608

    
609
    msg->full = 1;
610
    return PARSE_SUCCESS;
611

    
612
  case BABEL_AE_IP4:
613
    /* TODO */
614
    return PARSE_IGNORE;
615

    
616
  case BABEL_AE_IP6:
617
    if (tlv->plen > IP6_MAX_PREFIX_LENGTH)
618
      return PARSE_ERROR;
619

    
620
    if (TLV_OPT_LENGTH(tlv) < BYTES(tlv->plen))
621
      return PARSE_ERROR;
622

    
623
    read_ip6_px(&msg->net, tlv->addr, tlv->plen);
624
    return PARSE_SUCCESS;
625

    
626
  case BABEL_AE_IP6_LL:
627
    return PARSE_ERROR;
628

    
629
  default:
630
    return PARSE_IGNORE;
631
  }
632

    
633
  return PARSE_IGNORE;
634
}
635

    
636
static uint
637
babel_write_route_request(struct babel_tlv *hdr, union babel_msg *m,
638
                          struct babel_write_state *state UNUSED, uint max_len)
639
{
640
  struct babel_tlv_route_request *tlv = (void *) hdr;
641
  struct babel_msg_route_request *msg = &m->route_request;
642

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

    
645
  if (len > max_len)
646
    return 0;
647

    
648
  TLV_HDR(tlv, BABEL_TLV_ROUTE_REQUEST, len);
649

    
650
  if (msg->full)
651
  {
652
    tlv->ae = BABEL_AE_WILDCARD;
653
    tlv->plen = 0;
654
  }
655
  else
656
  {
657
    tlv->ae = BABEL_AE_IP6;
658
    tlv->plen = net6_pxlen(&msg->net);
659
    put_ip6_px(tlv->addr, &msg->net);
660
  }
661

    
662
  return len;
663
}
664

    
665
static int
666
babel_read_seqno_request(struct babel_tlv *hdr, union babel_msg *m,
667
                         struct babel_parse_state *state)
668
{
669
  struct babel_tlv_seqno_request *tlv = (void *) hdr;
670
  struct babel_msg_seqno_request *msg = &m->seqno_request;
671

    
672
  msg->type = BABEL_TLV_SEQNO_REQUEST;
673
  msg->seqno = get_u16(&tlv->seqno);
674
  msg->hop_count = tlv->hop_count;
675
  msg->router_id = get_u64(&tlv->router_id);
676
  msg->sender = state->saddr;
677

    
678
  if (tlv->hop_count == 0)
679
    return PARSE_ERROR;
680

    
681
  switch (tlv->ae)
682
  {
683
  case BABEL_AE_WILDCARD:
684
    return PARSE_ERROR;
685

    
686
  case BABEL_AE_IP4:
687
    /* TODO */
688
    return PARSE_IGNORE;
689

    
690
  case BABEL_AE_IP6:
691
    if (tlv->plen > IP6_MAX_PREFIX_LENGTH)
692
      return PARSE_ERROR;
693

    
694
    if (TLV_OPT_LENGTH(tlv) < BYTES(tlv->plen))
695
      return PARSE_ERROR;
696

    
697
    read_ip6_px(&msg->net, tlv->addr, tlv->plen);
698
    return PARSE_SUCCESS;
699

    
700
  case BABEL_AE_IP6_LL:
701
    return PARSE_ERROR;
702

    
703
  default:
704
    return PARSE_IGNORE;
705
  }
706

    
707
  return PARSE_IGNORE;
708
}
709

    
710
static uint
711
babel_write_seqno_request(struct babel_tlv *hdr, union babel_msg *m,
712
                          struct babel_write_state *state UNUSED, uint max_len)
713
{
714
  struct babel_tlv_seqno_request *tlv = (void *) hdr;
715
  struct babel_msg_seqno_request *msg = &m->seqno_request;
716

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

    
719
  if (len > max_len)
720
    return 0;
721

    
722
  TLV_HDR(tlv, BABEL_TLV_SEQNO_REQUEST, len);
723
  tlv->ae = BABEL_AE_IP6;
724
  tlv->plen = net6_pxlen(&msg->net);
725
  put_u16(&tlv->seqno, msg->seqno);
726
  tlv->hop_count = msg->hop_count;
727
  put_u64(&tlv->router_id, msg->router_id);
728
  put_ip6_px(tlv->addr, &msg->net);
729

    
730
  return len;
731
}
732

    
733
static inline int
734
babel_read_tlv(struct babel_tlv *hdr,
735
               union babel_msg *msg,
736
               struct babel_parse_state *state)
737
{
738
  if ((hdr->type <= BABEL_TLV_PADN) ||
739
      (hdr->type >= BABEL_TLV_MAX) ||
740
      !tlv_data[hdr->type].read_tlv)
741
    return PARSE_IGNORE;
742

    
743
  if (TLV_LENGTH(hdr) < tlv_data[hdr->type].min_length)
744
    return PARSE_ERROR;
745

    
746
  memset(msg, 0, sizeof(*msg));
747
  return tlv_data[hdr->type].read_tlv(hdr, msg, state);
748
}
749

    
750
static uint
751
babel_write_tlv(struct babel_tlv *hdr,
752
                union babel_msg *msg,
753
                struct babel_write_state *state,
754
                uint max_len)
755
{
756
  if ((msg->type <= BABEL_TLV_PADN) ||
757
      (msg->type >= BABEL_TLV_MAX) ||
758
      !tlv_data[msg->type].write_tlv)
759
    return 0;
760

    
761
  if (tlv_data[msg->type].min_length > max_len)
762
    return 0;
763

    
764
  memset(hdr, 0, tlv_data[msg->type].min_length);
765
  return tlv_data[msg->type].write_tlv(hdr, msg, state, max_len);
766
}
767

    
768

    
769
/*
770
 *        Packet RX/TX functions
771
 */
772

    
773
static int
774
babel_send_to(struct babel_iface *ifa, ip_addr dest)
775
{
776
  sock *sk = ifa->sk;
777
  struct babel_pkt_header *hdr = (void *) sk->tbuf;
778
  int len = get_u16(&hdr->length) + sizeof(struct babel_pkt_header);
779

    
780
  DBG("Babel: Sending %d bytes to %I\n", len, dest);
781
  return sk_send_to(sk, len, dest, 0);
782
}
783

    
784
/**
785
 * babel_write_queue - Write a TLV queue to a transmission buffer
786
 * @ifa: Interface holding the transmission buffer
787
 * @queue: TLV queue to write (containing internal-format TLVs)
788
 *
789
 * This function writes a packet to the interface transmission buffer with as
790
 * many TLVs from the &queue as will fit in the buffer. It returns the number of
791
 * bytes written (NOT counting the packet header). The function is called by
792
 * babel_send_queue() and babel_send_unicast() to construct packets for
793
 * transmission, and uses per-TLV helper functions to convert the
794
 * internal-format TLVs to their wire representations.
795
 *
796
 * The TLVs in the queue are freed after they are written to the buffer.
797
 */
798
static uint
799
babel_write_queue(struct babel_iface *ifa, list *queue)
800
{
801
  struct babel_proto *p = ifa->proto;
802
  struct babel_write_state state = {};
803

    
804
  if (EMPTY_LIST(*queue))
805
    return 0;
806

    
807
  byte *pos = ifa->sk->tbuf;
808
  byte *end = pos + ifa->tx_length;
809

    
810
  struct babel_pkt_header *pkt = (void *) pos;
811
  pkt->magic = BABEL_MAGIC;
812
  pkt->version = BABEL_VERSION;
813
  pkt->length = 0;
814
  pos += sizeof(struct babel_pkt_header);
815

    
816
  struct babel_msg_node *msg;
817
  WALK_LIST_FIRST(msg, *queue)
818
  {
819
    if (pos >= end)
820
      break;
821

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

    
824
    if (!len)
825
      break;
826

    
827
    pos += len;
828
    rem_node(NODE msg);
829
    sl_free(p->msg_slab, msg);
830
  }
831

    
832
  uint plen = pos - (byte *) pkt;
833
  put_u16(&pkt->length, plen - sizeof(struct babel_pkt_header));
834

    
835
  return plen;
836
}
837

    
838
void
839
babel_send_queue(void *arg)
840
{
841
  struct babel_iface *ifa = arg;
842
  while ((babel_write_queue(ifa, &ifa->msg_queue) > 0) &&
843
         (babel_send_to(ifa, IP6_BABEL_ROUTERS) > 0));
844
}
845

    
846
static inline void
847
babel_kick_queue(struct babel_iface *ifa)
848
{
849
  /*
850
   * Only schedule send event if there is not already data in the socket buffer.
851
   * Otherwise we may overwrite the data already in the buffer.
852
   */
853

    
854
  if ((ifa->sk->tpos == ifa->sk->tbuf) && !ev_active(ifa->send_event))
855
    ev_schedule(ifa->send_event);
856
}
857

    
858
/**
859
 * babel_send_unicast - send a single TLV via unicast to a destination
860
 * @msg: TLV to send
861
 * @ifa: Interface to send via
862
 * @dest: Destination of the TLV
863
 *
864
 * This function is used to send a single TLV via unicast to a designated
865
 * receiver. This is used for replying to certain incoming requests, and for
866
 * sending unicast requests to refresh routes before they expire.
867
 */
868
void
869
babel_send_unicast(union babel_msg *msg, struct babel_iface *ifa, ip_addr dest)
870
{
871
  struct babel_proto *p = ifa->proto;
872
  struct babel_msg_node *msgn = sl_alloc(p->msg_slab);
873
  list queue;
874

    
875
  msgn->msg = *msg;
876
  init_list(&queue);
877
  add_tail(&queue, NODE msgn);
878
  babel_write_queue(ifa, &queue);
879
  babel_send_to(ifa, dest);
880

    
881
  /* We could overwrite waiting packet here, we may have to kick TX queue */
882
  if (!EMPTY_LIST(ifa->msg_queue))
883
    babel_kick_queue(ifa);
884
}
885

    
886
/**
887
 * babel_enqueue - enqueue a TLV for transmission on an interface
888
 * @msg: TLV to enqueue (in internal TLV format)
889
 * @ifa: Interface to enqueue to
890
 *
891
 * This function is called to enqueue a TLV for subsequent transmission on an
892
 * interface. The transmission event is triggered whenever a TLV is enqueued;
893
 * this ensures that TLVs will be transmitted in a timely manner, but that TLVs
894
 * which are enqueued in rapid succession can be transmitted together in one
895
 * packet.
896
 */
897
void
898
babel_enqueue(union babel_msg *msg, struct babel_iface *ifa)
899
{
900
  struct babel_proto *p = ifa->proto;
901
  struct babel_msg_node *msgn = sl_alloc(p->msg_slab);
902
  msgn->msg = *msg;
903
  add_tail(&ifa->msg_queue, NODE msgn);
904
  babel_kick_queue(ifa);
905
}
906

    
907
/**
908
 * babel_process_packet - process incoming data packet
909
 * @pkt: Pointer to the packet data
910
 * @len: Length of received packet
911
 * @saddr: Address of packet sender
912
 * @ifa: Interface packet was received on.
913
 *
914
 * This function is the main processing hook of incoming Babel packets. It
915
 * checks that the packet header is well-formed, then processes the TLVs
916
 * contained in the packet. This is done in two passes: First all TLVs are
917
 * parsed into the internal TLV format. If a TLV parser fails, processing of the
918
 * rest of the packet is aborted.
919
 *
920
 * After the parsing step, the TLV handlers are called for each parsed TLV in
921
 * order.
922
 */
923
static void
924
babel_process_packet(struct babel_pkt_header *pkt, int len,
925
                     ip_addr saddr, struct babel_iface *ifa)
926
{
927
  struct babel_proto *p = ifa->proto;
928
  struct babel_tlv *tlv;
929
  struct babel_msg_node *msg;
930
  list msgs;
931
  int res;
932

    
933
  int plen = sizeof(struct babel_pkt_header) + get_u16(&pkt->length);
934
  byte *pos;
935
  byte *end = (byte *)pkt + plen;
936

    
937
  struct babel_parse_state state = {
938
    .proto        = p,
939
    .ifa        = ifa,
940
    .saddr        = saddr,
941
    .next_hop        = saddr,
942
  };
943

    
944
  if ((pkt->magic != BABEL_MAGIC) || (pkt->version != BABEL_VERSION))
945
  {
946
    TRACE(D_PACKETS, "Strange packet from %I via %s - magic %d version %d",
947
          saddr, ifa->iface->name, pkt->magic, pkt->version);
948
    return;
949
  }
950

    
951
  if (plen > len)
952
  {
953
    LOG_PKT("Bad packet from %I via %s - %s (%u)",
954
            saddr, ifa->iface->name, "length mismatch", plen);
955
    return;
956
  }
957

    
958
  TRACE(D_PACKETS, "Packet received from %I via %s",
959
        saddr, ifa->iface->name);
960

    
961
  init_list(&msgs);
962

    
963
  /* First pass through the packet TLV by TLV, parsing each into internal data
964
     structures. */
965
  for (tlv = FIRST_TLV(pkt);
966
       (byte *)tlv < end;
967
       tlv = NEXT_TLV(tlv))
968
  {
969
    /* Ugly special case */
970
    if (tlv->type == BABEL_TLV_PAD1)
971
      continue;
972

    
973
    /* The end of the common TLV header */
974
    pos = (byte *)tlv + sizeof(struct babel_tlv);
975
    if ((pos > end) || (pos + tlv->length > end))
976
    {
977
      LOG_PKT("Bad TLV from %I via %s type %d pos %d - framing error",
978
              saddr, ifa->iface->name, tlv->type, (byte *)tlv - (byte *)pkt);
979
      break;
980
    }
981

    
982
    msg = sl_alloc(p->msg_slab);
983
    res = babel_read_tlv(tlv, &msg->msg, &state);
984
    if (res == PARSE_SUCCESS)
985
    {
986
      add_tail(&msgs, NODE msg);
987
    }
988
    else if (res == PARSE_IGNORE)
989
    {
990
      DBG("Babel: Ignoring TLV of type %d\n", tlv->type);
991
      sl_free(p->msg_slab, msg);
992
    }
993
    else /* PARSE_ERROR */
994
    {
995
      LOG_PKT("Bad TLV from %I via %s type %d pos %d - parse error",
996
              saddr, ifa->iface->name, tlv->type, (byte *)tlv - (byte *)pkt);
997
      sl_free(p->msg_slab, msg);
998
      break;
999
    }
1000
  }
1001

    
1002
  /* Parsing done, handle all parsed TLVs */
1003
  WALK_LIST_FIRST(msg, msgs)
1004
  {
1005
    if (tlv_data[msg->msg.type].handle_tlv)
1006
      tlv_data[msg->msg.type].handle_tlv(&msg->msg, ifa);
1007
    rem_node(NODE msg);
1008
    sl_free(p->msg_slab, msg);
1009
  }
1010
}
1011

    
1012
static void
1013
babel_err_hook(sock *sk, int err)
1014
{
1015
  struct babel_iface *ifa = sk->data;
1016
  struct babel_proto *p = ifa->proto;
1017

    
1018
  log(L_ERR "%s: Socket error on %s: %M", p->p.name, ifa->iface->name, err);
1019
  /* FIXME: Drop queued TLVs here? */
1020
}
1021

    
1022

    
1023
static void
1024
babel_tx_hook(sock *sk)
1025
{
1026
  struct babel_iface *ifa = sk->data;
1027

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

    
1031
  babel_send_queue(ifa);
1032
}
1033

    
1034

    
1035
static int
1036
babel_rx_hook(sock *sk, uint len)
1037
{
1038
  struct babel_iface *ifa = sk->data;
1039
  struct babel_proto *p = ifa->proto;
1040
  const char *err_dsc = NULL;
1041
  uint err_val = 0;
1042

    
1043
  if (sk->lifindex != ifa->iface->index)
1044
    return 1;
1045

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

    
1049
  /* Silently ignore my own packets */
1050
  if (ipa_equal(ifa->iface->addr->ip, sk->faddr))
1051
    return 1;
1052

    
1053
  if (!ipa_is_link_local(sk->faddr))
1054
    DROP1("wrong src address");
1055

    
1056
  if (sk->fport != ifa->cf->port)
1057
    DROP("wrong src port", sk->fport);
1058

    
1059
  if (len < sizeof(struct babel_pkt_header))
1060
    DROP("too short", len);
1061

    
1062
  if (sk->flags & SKF_TRUNCATED)
1063
    DROP("truncated", len);
1064

    
1065
  babel_process_packet((struct babel_pkt_header *) sk->rbuf, len, sk->faddr, ifa);
1066
  return 1;
1067

    
1068
drop:
1069
  LOG_PKT("Bad packet from %I via %s - %s (%u)",
1070
          sk->faddr, sk->iface->name, err_dsc, err_val);
1071
  return 1;
1072
}
1073

    
1074
int
1075
babel_open_socket(struct babel_iface *ifa)
1076
{
1077
  struct babel_proto *p = ifa->proto;
1078

    
1079
  sock *sk;
1080
  sk = sk_new(ifa->pool);
1081
  sk->type = SK_UDP;
1082
  sk->sport = ifa->cf->port;
1083
  sk->dport = ifa->cf->port;
1084
  sk->iface = ifa->iface;
1085

    
1086
  sk->rx_hook = babel_rx_hook;
1087
  sk->tx_hook = babel_tx_hook;
1088
  sk->err_hook = babel_err_hook;
1089
  sk->data = ifa;
1090

    
1091
  sk->tos = ifa->cf->tx_tos;
1092
  sk->priority = ifa->cf->tx_priority;
1093
  sk->ttl = 1;
1094
  sk->flags = SKF_LADDR_RX;
1095

    
1096
  if (sk_open(sk) < 0)
1097
    goto err;
1098

    
1099
  if (sk_setup_multicast(sk) < 0)
1100
    goto err;
1101

    
1102
  if (sk_join_group(sk, IP6_BABEL_ROUTERS) < 0)
1103
    goto err;
1104

    
1105
  ifa->sk = sk;
1106
  return 1;
1107

    
1108
err:
1109
  sk_log_error(sk, p->p.name);
1110
  rfree(sk);
1111
  return 0;
1112
}