proto/babel/packets.c

   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
 150 static inline u16
 151 get_time16(const void *p)
 152 {
 153   u16 v = get_u16(p) / BABEL_TIME_UNITS;
 154   return MAX(1, v);
 155 }
 156
 157 static inline void
 158 put_time16(void *p, u16 v)
 159 {
 160   put_u16(p, v * BABEL_TIME_UNITS);
 161 }
 162
 163 static inline ip6_addr
 164 get_ip6_px(const void *p, int plen)
 165 {
 166   ip6_addr addr = IPA_NONE;
 167   memcpy(&addr, p, (plen + 7) / 8);
 168   return ip6_ntoh(addr);
 169 }
 170
 171 static inline void
 172 put_ip6_px(void *p, ip6_addr addr, int plen)
 173 {
 174   addr = ip6_hton(addr);
 175   memcpy(p, &addr, (plen + 7) / 8);
 176 }
 177
 178 static inline ip6_addr
 179 get_ip6_ll(const void *p)
 180 {
 181   return ip6_build(0xfe800000, 0, get_u32(p+0), get_u32(p+4));
 182 }
 183
 184 static inline void
 185 put_ip6_ll(void *p, ip6_addr addr)
 186 {
 187   put_u32(p+0, _I2(addr));
 188   put_u32(p+4, _I3(addr));
 189 }
 190
 191
 192 /*
 193  *      TLV read/write functions
 194  */
 195
 196 static int babel_read_ack_req(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
 197 static int babel_read_hello(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
 198 static int babel_read_ihu(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
 199 static int babel_read_router_id(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
 200 static int babel_read_next_hop(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
 201 static int babel_read_update(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
 202 static int babel_read_route_request(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
 203 static int babel_read_seqno_request(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
 204
 205 static int babel_write_ack(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, int max_len);
 206 static int babel_write_hello(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, int max_len);
 207 static int babel_write_ihu(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, int max_len);
 208 static int babel_write_update(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, int max_len);
 209 static int babel_write_route_request(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, int max_len);
 210 static int babel_write_seqno_request(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, int max_len);
 211
 212 struct babel_tlv_data {
 213   u8 min_length;
 214   int (*read_tlv)(struct babel_tlv *hdr, union babel_msg *m, struct babel_parse_state *state);
 215   int (*write_tlv)(struct babel_tlv *hdr, union babel_msg *m, struct babel_write_state *state, int max_len);
 216   void (*handle_tlv)(union babel_msg *m, struct babel_iface *ifa);
 217 };
 218
 219 const static struct babel_tlv_data tlv_data[BABEL_TLV_MAX] = {
 220   [BABEL_TLV_ACK_REQ] = {
 221     sizeof(struct babel_tlv_ack_req),
 222     babel_read_ack_req,
 223     NULL,
 224     babel_handle_ack_req
 225   },
 226   [BABEL_TLV_ACK] = {
 227     sizeof(struct babel_tlv_ack),
 228     NULL,
 229     babel_write_ack,
 230     NULL
 231   },
 232   [BABEL_TLV_HELLO] = {
 233     sizeof(struct babel_tlv_hello),
 234     babel_read_hello,
 235     babel_write_hello,
 236     babel_handle_hello
 237   },
 238   [BABEL_TLV_IHU] = {
 239     sizeof(struct babel_tlv_ihu),
 240     babel_read_ihu,
 241     babel_write_ihu,
 242     babel_handle_ihu
 243   },
 244   [BABEL_TLV_ROUTER_ID] = {
 245     sizeof(struct babel_tlv_router_id),
 246     babel_read_router_id,
 247     NULL,
 248     NULL
 249   },
 250   [BABEL_TLV_NEXT_HOP] = {
 251     sizeof(struct babel_tlv_next_hop),
 252     babel_read_next_hop,
 253     NULL,
 254     NULL
 255   },
 256   [BABEL_TLV_UPDATE] = {
 257     sizeof(struct babel_tlv_update),
 258     babel_read_update,
 259     babel_write_update,
 260     babel_handle_update
 261   },
 262   [BABEL_TLV_ROUTE_REQUEST] = {
 263     sizeof(struct babel_tlv_route_request),
 264     babel_read_route_request,
 265     babel_write_route_request,
 266     babel_handle_route_request
 267   },
 268   [BABEL_TLV_SEQNO_REQUEST] = {
 269     sizeof(struct babel_tlv_seqno_request),
 270     babel_read_seqno_request,
 271     babel_write_seqno_request,
 272     babel_handle_seqno_request
 273   },
 274 };
 275
 276 static int
 277 babel_read_ack_req(struct babel_tlv *hdr, union babel_msg *m,
 278                    struct babel_parse_state *state)
 279 {
 280   struct babel_tlv_ack_req *tlv = (void *) hdr;
 281   struct babel_msg_ack_req *msg = &m->ack_req;
 282
 283   msg->type = BABEL_TLV_ACK_REQ;
 284   msg->nonce = get_u16(&tlv->nonce);
 285   msg->interval = get_time16(&tlv->interval);
 286   msg->sender = state->saddr;
 287
 288   if (!msg->interval)
 289     return PARSE_ERROR;
 290
 291   return PARSE_SUCCESS;
 292 }
 293
 294 static int
 295 babel_write_ack(struct babel_tlv *hdr, union babel_msg *m,
 296                 struct babel_write_state *state, int max_len)
 297 {
 298   struct babel_tlv_ack *tlv = (void *) hdr;
 299   struct babel_msg_ack *msg = &m->ack;
 300
 301   TLV_HDR0(tlv, BABEL_TLV_ACK);
 302   put_u16(&tlv->nonce, msg->nonce);
 303
 304   return sizeof(struct babel_tlv_ack);
 305 }
 306
 307 static int
 308 babel_read_hello(struct babel_tlv *hdr, union babel_msg *m,
 309                  struct babel_parse_state *state)
 310 {
 311   struct babel_tlv_hello *tlv = (void *) hdr;
 312   struct babel_msg_hello *msg = &m->hello;
 313
 314   msg->type = BABEL_TLV_HELLO;
 315   msg->seqno = get_u16(&tlv->seqno);
 316   msg->interval = get_time16(&tlv->interval);
 317   msg->sender = state->saddr;
 318
 319   return PARSE_SUCCESS;
 320 }
 321
 322 static int
 323 babel_write_hello(struct babel_tlv *hdr, union babel_msg *m,
 324                   struct babel_write_state *state, int max_len)
 325 {
 326   struct babel_tlv_hello *tlv = (void *) hdr;
 327   struct babel_msg_hello *msg = &m->hello;
 328
 329   TLV_HDR0(tlv, BABEL_TLV_HELLO);
 330   put_u16(&tlv->seqno, msg->seqno);
 331   put_time16(&tlv->interval, msg->interval);
 332
 333   return sizeof(struct babel_tlv_hello);
 334 }
 335
 336 static int
 337 babel_read_ihu(struct babel_tlv *hdr, union babel_msg *m,
 338                struct babel_parse_state *state)
 339 {
 340   struct babel_tlv_ihu *tlv = (void *) hdr;
 341   struct babel_msg_ihu *msg = &m->ihu;
 342
 343   msg->type = BABEL_TLV_IHU;
 344   msg->ae = tlv->ae;
 345   msg->rxcost = get_u16(&tlv->rxcost);
 346   msg->interval = get_time16(&tlv->interval);
 347   msg->addr = IPA_NONE;
 348   msg->sender = state->saddr;
 349
 350   if (msg->ae >= BABEL_AE_MAX)
 351     return PARSE_IGNORE;
 352
 353   // We handle link-local IPs. In every other case, the addr field will be 0 but
 354   // validation will succeed. The handler takes care of these cases.
 355   if (msg->ae == BABEL_AE_IP6_LL)
 356   {
 357     if (TLV_OPT_LENGTH(tlv) < 8)
 358       return PARSE_ERROR;
 359
 360     msg->addr = ipa_from_ip6(get_ip6_ll(&tlv->addr));
 361   }
 362
 363   return PARSE_SUCCESS;
 364 }
 365
 366 static int
 367 babel_write_ihu(struct babel_tlv *hdr, union babel_msg *m,
 368                 struct babel_write_state *state, int max_len)
 369 {
 370   struct babel_tlv_ihu *tlv = (void *) hdr;
 371   struct babel_msg_ihu *msg = &m->ihu;
 372
 373   if (ipa_is_link_local(msg->addr) && max_len < sizeof(struct babel_tlv_ihu) + 8)
 374     return 0;
 375
 376   TLV_HDR0(tlv, BABEL_TLV_IHU);
 377   put_u16(&tlv->rxcost, msg->rxcost);
 378   put_time16(&tlv->interval, msg->interval);
 379
 380   if (!ipa_is_link_local(msg->addr))
 381   {
 382     tlv->ae = BABEL_AE_WILDCARD;
 383     return sizeof(struct babel_tlv_ihu);
 384   }
 385   put_ip6_ll(&tlv->addr, msg->addr);
 386   tlv->ae = BABEL_AE_IP6_LL;
 387   hdr->length += 8;
 388   return sizeof(struct babel_tlv_ihu) + 8;
 389 }
 390
 391 static int
 392 babel_read_router_id(struct babel_tlv *hdr, union babel_msg *m UNUSED,
 393                      struct babel_parse_state *state)
 394 {
 395   struct babel_tlv_router_id *tlv = (void *) hdr;
 396
 397   state->router_id = get_u64(&tlv->router_id);
 398   state->router_id_seen = 1;
 399
 400   return PARSE_IGNORE;
 401 }
 402
 403 /* This is called directly from babel_write_update() */
 404 static int
 405 babel_write_router_id(struct babel_tlv *hdr, u64 router_id,
 406                       struct babel_write_state *state, int max_len UNUSED)
 407 {
 408   struct babel_tlv_router_id *tlv = (void *) hdr;
 409
 410   /* We still assume that first min_length bytes are available and zeroed */
 411
 412   TLV_HDR0(tlv, BABEL_TLV_ROUTER_ID);
 413   put_u64(&tlv->router_id, router_id);
 414
 415   state->router_id = router_id;
 416   state->router_id_seen = 1;
 417
 418   return sizeof(struct babel_tlv_router_id);
 419 }
 420
 421 static int
 422 babel_read_next_hop(struct babel_tlv *hdr, union babel_msg *m UNUSED,
 423                     struct babel_parse_state *state)
 424 {
 425   struct babel_tlv_next_hop *tlv = (void *) hdr;
 426
 427   switch (tlv->ae)
 428   {
 429   case BABEL_AE_WILDCARD:
 430     return PARSE_ERROR;
 431
 432   case BABEL_AE_IP4:
 433     /* TODO */
 434     return PARSE_IGNORE;
 435
 436   case BABEL_AE_IP6:
 437     if (TLV_OPT_LENGTH(tlv) < sizeof(ip6_addr))
 438       return PARSE_ERROR;
 439
 440     state->next_hop = ipa_from_ip6(get_ip6(&tlv->addr));
 441     return PARSE_IGNORE;
 442
 443   case BABEL_AE_IP6_LL:
 444     if (TLV_OPT_LENGTH(tlv) < 8)
 445       return PARSE_ERROR;
 446
 447     state->next_hop = ipa_from_ip6(get_ip6_ll(&tlv->addr));
 448     return PARSE_IGNORE;
 449
 450   default:
 451     return PARSE_IGNORE;
 452   }
 453
 454   return PARSE_IGNORE;
 455 }
 456
 457 static int
 458 babel_read_update(struct babel_tlv *hdr, union babel_msg *m,
 459                   struct babel_parse_state *state)
 460 {
 461   struct babel_tlv_update *tlv = (void *) hdr;
 462   struct babel_msg_update *msg = &m->update;
 463
 464   msg->type = BABEL_TLV_UPDATE;
 465   msg->ae = tlv->ae;
 466   msg->interval = get_time16(&tlv->interval);
 467   msg->seqno = get_u16(&tlv->seqno);
 468   msg->metric = get_u16(&tlv->metric);
 469
 470   /* Length of received prefix data without omitted part */
 471   int len = (tlv->plen + 7)/8 - (int) tlv->omitted;
 472   u8 buf[16] = {};
 473
 474   if ((len < 0) || (len > TLV_OPT_LENGTH(tlv)))
 475     return PARSE_ERROR;
 476
 477   switch (tlv->ae)
 478   {
 479   case BABEL_AE_WILDCARD:
 480     if (tlv->plen > 0)
 481       return PARSE_ERROR;
 482
 483     msg->prefix = IPA_NONE;
 484     break;
 485
 486   case BABEL_AE_IP4:
 487     /* TODO */
 488     return PARSE_IGNORE;
 489
 490   case BABEL_AE_IP6:
 491     if (tlv->plen > MAX_PREFIX_LENGTH)
 492       return PARSE_ERROR;
 493
 494     /* Cannot omit data if there is no saved prefix */
 495     if (tlv->omitted && !state->def_ip6_prefix_seen)
 496       return PARSE_ERROR;
 497
 498     /* Merge saved prefix and received prefix parts */
 499     memcpy(buf, state->def_ip6_prefix, tlv->omitted);
 500     memcpy(buf + tlv->omitted, tlv->addr, len);
 501
 502     msg->plen = tlv->plen;
 503     msg->prefix = ipa_from_ip6(get_ip6(buf));
 504
 505     if (tlv->flags & BABEL_FLAG_DEF_PREFIX)
 506     {
 507       put_ip6(state->def_ip6_prefix, msg->prefix);
 508       state->def_ip6_prefix_seen = 1;
 509     }
 510
 511     if (tlv->flags & BABEL_FLAG_ROUTER_ID)
 512     {
 513       state->router_id = ((u64) _I2(msg->prefix)) << 32 | _I3(msg->prefix);
 514       state->router_id_seen = 1;
 515     }
 516     break;
 517
 518   case BABEL_AE_IP6_LL:
 519     /* ??? */
 520     return PARSE_IGNORE;
 521
 522   default:
 523     return PARSE_IGNORE;
 524   }
 525
 526   if (!state->router_id_seen)
 527   {
 528     DBG("Babel: No router ID seen before update\n");
 529     return PARSE_ERROR;
 530   }
 531
 532   msg->router_id = state->router_id;
 533   msg->next_hop = state->next_hop;
 534   msg->sender = state->saddr;
 535
 536   return PARSE_SUCCESS;
 537 }
 538
 539 static int
 540 babel_write_update(struct babel_tlv *hdr, union babel_msg *m,
 541                    struct babel_write_state *state, int max_len)
 542 {
 543   struct babel_tlv_update *tlv = (void *) hdr;
 544   struct babel_msg_update *msg = &m->update;
 545   int len0 = 0;
 546
 547   /*
 548    * When needed, we write Router-ID TLV before Update TLV and return size of
 549    * both of them. There is enough space for the Router-ID TLV, because
 550    * sizeof(struct babel_tlv_router_id) == sizeof(struct babel_tlv_update).
 551    */
 552   if (!state->router_id_seen || (msg->router_id != state->router_id))
 553   {
 554     len0 = babel_write_router_id(hdr, msg->router_id, state, max_len);
 555     tlv = (struct babel_tlv_update *) NEXT_TLV(tlv);
 556   }
 557
 558   int len = sizeof(struct babel_tlv_update) + (msg->plen + 7)/8;
 559
 560   if (len0 + len > max_len)
 561     return 0;
 562
 563   memset(tlv, 0, sizeof(struct babel_tlv_update));
 564   TLV_HDR(tlv, BABEL_TLV_UPDATE, len);
 565   tlv->ae = BABEL_AE_IP6;
 566   tlv->plen = msg->plen;
 567   put_time16(&tlv->interval, msg->interval);
 568   put_u16(&tlv->seqno, msg->seqno);
 569   put_u16(&tlv->metric, msg->metric);
 570   put_ip6_px(tlv->addr, msg->prefix, msg->plen);
 571
 572   return len0 + len;
 573 }
 574
 575 static int
 576 babel_read_route_request(struct babel_tlv *hdr, union babel_msg *m,
 577                          struct babel_parse_state *state)
 578 {
 579   struct babel_tlv_route_request *tlv = (void *) hdr;
 580   struct babel_msg_route_request *msg = &m->route_request;
 581
 582   msg->type = BABEL_TLV_ROUTE_REQUEST;
 583
 584   switch (tlv->ae)
 585   {
 586   case BABEL_AE_WILDCARD:
 587     /* Wildcard requests must have plen 0 */
 588     if (tlv->plen > 0)
 589       return PARSE_ERROR;
 590
 591     msg->full = 1;
 592     return PARSE_SUCCESS;
 593
 594   case BABEL_AE_IP4:
 595     /* TODO */
 596     return PARSE_IGNORE;
 597
 598   case BABEL_AE_IP6:
 599     if (tlv->plen > MAX_PREFIX_LENGTH)
 600       return PARSE_ERROR;
 601
 602     if (TLV_OPT_LENGTH(tlv) < (tlv->plen + 7)/8)
 603       return PARSE_ERROR;
 604
 605     msg->plen = tlv->plen;
 606     msg->prefix = get_ip6_px(tlv->addr, tlv->plen);
 607     return PARSE_SUCCESS;
 608
 609   case BABEL_AE_IP6_LL:
 610     return PARSE_ERROR;
 611
 612   default:
 613     return PARSE_IGNORE;
 614   }
 615
 616   return PARSE_IGNORE;
 617 }
 618
 619 static int
 620 babel_write_route_request(struct babel_tlv *hdr, union babel_msg *m,
 621                           struct babel_write_state *state, int max_len)
 622 {
 623   struct babel_tlv_route_request *tlv = (void *) hdr;
 624   struct babel_msg_route_request *msg = &m->route_request;
 625
 626   int len = sizeof(struct babel_tlv_route_request) + (msg->plen + 7)/8;
 627
 628   if (len > max_len)
 629     return 0;
 630
 631   TLV_HDR(tlv, BABEL_TLV_ROUTE_REQUEST, len);
 632
 633   if (msg->full)
 634   {
 635     tlv->ae = BABEL_AE_WILDCARD;
 636     tlv->plen = 0;
 637   }
 638   else
 639   {
 640     tlv->ae = BABEL_AE_IP6;
 641     tlv->plen = msg->plen;
 642     put_ip6_px(tlv->addr, msg->prefix, msg->plen);
 643   }
 644
 645   return len;
 646 }
 647
 648 static int
 649 babel_read_seqno_request(struct babel_tlv *hdr, union babel_msg *m,
 650                          struct babel_parse_state *state)
 651 {
 652   struct babel_tlv_seqno_request *tlv = (void *) hdr;
 653   struct babel_msg_seqno_request *msg = &m->seqno_request;
 654
 655   msg->type = BABEL_TLV_SEQNO_REQUEST;
 656   msg->seqno = get_u16(&tlv->seqno);
 657   msg->hop_count = tlv->hop_count;
 658   msg->router_id = get_u64(&tlv->router_id);
 659   msg->sender = state->saddr;
 660
 661   if (tlv->hop_count == 0)
 662     return PARSE_ERROR;
 663
 664   switch (tlv->ae)
 665   {
 666   case BABEL_AE_WILDCARD:
 667     return PARSE_ERROR;
 668
 669   case BABEL_AE_IP4:
 670     /* TODO */
 671     return PARSE_IGNORE;
 672
 673   case BABEL_AE_IP6:
 674     if (tlv->plen > MAX_PREFIX_LENGTH)
 675       return PARSE_ERROR;
 676
 677     if (TLV_OPT_LENGTH(tlv) < (tlv->plen + 7)/8)
 678       return PARSE_ERROR;
 679
 680     msg->plen = tlv->plen;
 681     msg->prefix = get_ip6_px(tlv->addr, tlv->plen);
 682     return PARSE_SUCCESS;
 683
 684   case BABEL_AE_IP6_LL:
 685     return PARSE_ERROR;
 686
 687   default:
 688     return PARSE_IGNORE;
 689   }
 690
 691   return PARSE_IGNORE;
 692 }
 693
 694 static int
 695 babel_write_seqno_request(struct babel_tlv *hdr, union babel_msg *m,
 696                           struct babel_write_state *state, int max_len)
 697 {
 698   struct babel_tlv_seqno_request *tlv = (void *) hdr;
 699   struct babel_msg_seqno_request *msg = &m->seqno_request;
 700
 701   int len = sizeof(struct babel_tlv_seqno_request) + (msg->plen + 7)/8;
 702
 703   if (len > max_len)
 704     return 0;
 705
 706   TLV_HDR(tlv, BABEL_TLV_SEQNO_REQUEST, len);
 707   tlv->ae = BABEL_AE_IP6;
 708   tlv->plen = msg->plen;
 709   put_u16(&tlv->seqno, msg->seqno);
 710   tlv->hop_count = msg->hop_count;
 711   put_u64(&tlv->router_id, msg->router_id);
 712   put_ip6_px(tlv->addr, msg->prefix, msg->plen);
 713
 714   return len;
 715 }
 716
 717 static inline int
 718 babel_read_tlv(struct babel_tlv *hdr,
 719                union babel_msg *msg,
 720                struct babel_parse_state *state)
 721 {
 722   if ((hdr->type <= BABEL_TLV_PADN) ||
 723       (hdr->type >= BABEL_TLV_MAX) ||
 724       !tlv_data[hdr->type].read_tlv)
 725     return PARSE_IGNORE;
 726
 727   if (TLV_LENGTH(hdr) < tlv_data[hdr->type].min_length)
 728     return PARSE_ERROR;
 729
 730   memset(msg, 0, sizeof(*msg));
 731   return tlv_data[hdr->type].read_tlv(hdr, msg, state);
 732 }
 733
 734 static int
 735 babel_write_tlv(struct babel_tlv *hdr,
 736                 union babel_msg *msg,
 737                 struct babel_write_state *state,
 738                 int max_len)
 739 {
 740   if ((msg->type <= BABEL_TLV_PADN) ||
 741       (msg->type >= BABEL_TLV_MAX) ||
 742       !tlv_data[msg->type].write_tlv)
 743     return 0;
 744
 745   if (tlv_data[msg->type].min_length > max_len)
 746     return 0;
 747
 748   memset(hdr, 0, tlv_data[msg->type].min_length);
 749   return tlv_data[msg->type].write_tlv(hdr, msg, state, max_len);
 750 }
 751
 752
 753 /*
 754  *      Packet RX/TX functions
 755  */
 756
 757 static int
 758 babel_send_to(struct babel_iface *ifa, ip_addr dest)
 759 {
 760   sock *sk = ifa->sk;
 761   struct babel_pkt_header *hdr = (void *) sk->tbuf;
 762   int len = get_u16(&hdr->length) + sizeof(struct babel_pkt_header);
 763
 764   DBG("Babel: Sending %d bytes to %I\n", len, dest);
 765   return sk_send_to(sk, len, dest, 0);
 766 }
 767
 768 /**
 769  * babel_write_queue - Write a TLV queue to a transmission buffer
 770  * @ifa: Interface holding the transmission buffer
 771  * @queue: TLV queue to write (containing internal-format TLVs)
 772  *
 773  * This function writes a packet to the interface transmission buffer with as
 774  * many TLVs from the &queue as will fit in the buffer. It returns the number of
 775  * bytes written (NOT counting the packet header). The function is called by
 776  * babel_send_queue() and babel_send_unicast() to construct packets for
 777  * transmission, and uses per-TLV helper functions to convert the
 778  * internal-format TLVs to their wire representations.
 779  *
 780  * The TLVs in the queue are freed after they are written to the buffer.
 781  */
 782 static int
 783 babel_write_queue(struct babel_iface *ifa, list *queue)
 784 {
 785   struct babel_proto *p = ifa->proto;
 786   struct babel_write_state state = {};
 787
 788   if (EMPTY_LIST(*queue))
 789     return 0;
 790
 791   byte *pos = ifa->sk->tbuf;
 792   byte *end = pos + ifa->tx_length;
 793
 794   struct babel_pkt_header *pkt = (void *) pos;
 795   pkt->magic = BABEL_MAGIC;
 796   pkt->version = BABEL_VERSION;
 797   pkt->length = 0;
 798   pos += sizeof(struct babel_pkt_header);
 799
 800   struct babel_msg_node *msg;
 801   WALK_LIST_FIRST(msg, *queue)
 802   {
 803     int len = babel_write_tlv((struct babel_tlv *) pos, &msg->msg, &state, end - pos);
 804
 805     if (!len)
 806       break;
 807
 808     pos += len;
 809     rem_node(NODE msg);
 810     sl_free(p->msg_slab, msg);
 811   }
 812
 813   int plen = pos - (byte *) pkt;
 814   put_u16(&pkt->length, plen - sizeof(struct babel_pkt_header));
 815
 816   return plen;
 817 }
 818
 819 void
 820 babel_send_queue(void *arg)
 821 {
 822   struct babel_iface *ifa = arg;
 823   while ((babel_write_queue(ifa, &ifa->msg_queue) > 0) &&
 824          (babel_send_to(ifa, IP6_BABEL_ROUTERS) > 0));
 825 }
 826
 827 static inline void
 828 babel_kick_queue(struct babel_iface *ifa)
 829 {
 830   /*
 831    * Only schedule send event if there is not already data in the socket buffer.
 832    * Otherwise we may overwrite the data already in the buffer.
 833    */
 834
 835   if ((ifa->sk->tpos == ifa->sk->tbuf) && !ev_active(ifa->send_event))
 836     ev_schedule(ifa->send_event);
 837 }
 838
 839 /**
 840  * babel_send_unicast - send a single TLV via unicast to a destination
 841  * @msg: TLV to send
 842  * @ifa: Interface to send via
 843  * @dest: Destination of the TLV
 844  *
 845  * This function is used to send a single TLV via unicast to a designated
 846  * receiver. This is used for replying to certain incoming requests, and for
 847  * sending unicast requests to refresh routes before they expire.
 848  */
 849 void
 850 babel_send_unicast(union babel_msg *msg, struct babel_iface *ifa, ip_addr dest)
 851 {
 852   struct babel_proto *p = ifa->proto;
 853   struct babel_msg_node *msgn = sl_alloc(p->msg_slab);
 854   list queue;
 855
 856   msgn->msg = *msg;
 857   init_list(&queue);
 858   add_tail(&queue, NODE msgn);
 859   babel_write_queue(ifa, &queue);
 860   babel_send_to(ifa, dest);
 861
 862   /* We could overwrite waiting packet here, we may have to kick TX queue */
 863   if (!EMPTY_LIST(ifa->msg_queue))
 864     babel_kick_queue(ifa);
 865 }
 866
 867 /**
 868  * babel_enqueue - enqueue a TLV for transmission on an interface
 869  * @msg: TLV to enqueue (in internal TLV format)
 870  * @ifa: Interface to enqueue to
 871  *
 872  * This function is called to enqueue a TLV for subsequent transmission on an
 873  * interface. The transmission event is triggered whenever a TLV is enqueued;
 874  * this ensures that TLVs will be transmitted in a timely manner, but that TLVs
 875  * which are enqueued in rapid succession can be transmitted together in one
 876  * packet.
 877  */
 878 void
 879 babel_enqueue(union babel_msg *msg, struct babel_iface *ifa)
 880 {
 881   struct babel_proto *p = ifa->proto;
 882   struct babel_msg_node *msgn = sl_alloc(p->msg_slab);
 883   msgn->msg = *msg;
 884   add_tail(&ifa->msg_queue, NODE msgn);
 885   babel_kick_queue(ifa);
 886 }
 887
 888 /**
 889  * babel_process_packet - process incoming data packet
 890  * @pkt: Pointer to the packet data
 891  * @len: Length of received packet
 892  * @saddr: Address of packet sender
 893  * @ifa: Interface packet was received on.
 894  *
 895  * This function is the main processing hook of incoming Babel packets. It
 896  * checks that the packet header is well-formed, then processes the TLVs
 897  * contained in the packet. This is done in two passes: First all TLVs are
 898  * parsed into the internal TLV format. If a TLV parser fails, processing of the
 899  * rest of the packet is aborted.
 900  *
 901  * After the parsing step, the TLV handlers are called for each parsed TLV in
 902  * order.
 903  */
 904 static void
 905 babel_process_packet(struct babel_pkt_header *pkt, int len,
 906                      ip_addr saddr, struct babel_iface *ifa)
 907 {
 908   struct babel_proto *p = ifa->proto;
 909   struct babel_tlv *tlv;
 910   struct babel_msg_node *msg;
 911   list msgs;
 912   int res;
 913
 914   int plen = sizeof(struct babel_pkt_header) + get_u16(&pkt->length);
 915   byte *pos;
 916   byte *end = (byte *)pkt + plen;
 917
 918   struct babel_parse_state state = {
 919     .proto      = p,
 920     .ifa        = ifa,
 921     .saddr      = saddr,
 922     .next_hop   = saddr,
 923   };
 924
 925   if ((pkt->magic != BABEL_MAGIC) || (pkt->version != BABEL_VERSION))
 926   {
 927     TRACE(D_PACKETS, "Strange packet from %I via %s - magic %d version %d",
 928           saddr, ifa->iface->name, pkt->magic, pkt->version);
 929     return;
 930   }
 931
 932   if (plen > len)
 933   {
 934     LOG_PKT("Bad packet from %I via %s - %s (%u)",
 935             saddr, ifa->iface->name, "length mismatch", plen);
 936     return;
 937   }
 938
 939   TRACE(D_PACKETS, "Packet received from %I via %s",
 940         saddr, ifa->iface->name);
 941
 942   init_list(&msgs);
 943
 944   /* First pass through the packet TLV by TLV, parsing each into internal data
 945      structures. */
 946   for (tlv = FIRST_TLV(pkt);
 947        (byte *)tlv < end;
 948        tlv = NEXT_TLV(tlv))
 949   {
 950     /* Ugly special case */
 951     if (tlv->type == BABEL_TLV_PAD1)
 952       continue;
 953
 954     /* The end of the common TLV header */
 955     pos = (byte *)tlv + sizeof(struct babel_tlv);
 956     if ((pos > end) || (pos + tlv->length > end))
 957     {
 958       LOG_PKT("Bad TLV from %I via %s type %d pos %d - framing error",
 959               saddr, ifa->iface->name, tlv->type, (byte *)tlv - (byte *)pkt);
 960       break;
 961     }
 962
 963     msg = sl_alloc(p->msg_slab);
 964     res = babel_read_tlv(tlv, &msg->msg, &state);
 965     if (res == PARSE_SUCCESS)
 966     {
 967       add_tail(&msgs, NODE msg);
 968     }
 969     else if (res == PARSE_IGNORE)
 970     {
 971       DBG("Babel: Ignoring TLV of type %d\n", tlv->type);
 972       sl_free(p->msg_slab, msg);
 973     }
 974     else /* PARSE_ERROR */
 975     {
 976       LOG_PKT("Bad TLV from %I via %s type %d pos %d - parse error",
 977               saddr, ifa->iface->name, tlv->type, (byte *)tlv - (byte *)pkt);
 978       sl_free(p->msg_slab, msg);
 979       break;
 980     }
 981   }
 982
 983   /* Parsing done, handle all parsed TLVs */
 984   WALK_LIST_FIRST(msg, msgs)
 985   {
 986     if (tlv_data[msg->msg.type].handle_tlv)
 987       tlv_data[msg->msg.type].handle_tlv(&msg->msg, ifa);
 988     rem_node(NODE msg);
 989     sl_free(p->msg_slab, msg);
 990   }
 991 }
 992
 993 static void
 994 babel_err_hook(sock *sk, int err)
 995 {
 996   struct babel_iface *ifa = sk->data;
 997   struct babel_proto *p = ifa->proto;
 998
 999   log(L_ERR "%s: Socket error on %s: %M", p->p.name, ifa->iface->name, err);
1000   /* FIXME: Drop queued TLVs here? */
1001 }
1002
1003
1004 static void
1005 babel_tx_hook(sock *sk)
1006 {
1007   struct babel_iface *ifa = sk->data;
1008
1009   DBG("Babel: TX hook called (iface %s, src %I, dst %I)\n",
1010       sk->iface->name, sk->saddr, sk->daddr);
1011
1012   babel_send_queue(ifa);
1013 }
1014
1015
1016 static int
1017 babel_rx_hook(sock *sk, int len)
1018 {
1019   struct babel_iface *ifa = sk->data;
1020   struct babel_proto *p = ifa->proto;
1021   const char *err_dsc = NULL;
1022   uint err_val = 0;
1023
1024   if (sk->lifindex != ifa->iface->index)
1025     return 1;
1026
1027   DBG("Babel: RX hook called (iface %s, src %I, dst %I)\n",
1028       sk->iface->name, sk->faddr, sk->laddr);
1029
1030   /* Silently ignore my own packets */
1031   if (ipa_equal(ifa->iface->addr->ip, sk->faddr))
1032     return 1;
1033
1034   if (!ipa_is_link_local(sk->faddr))
1035     DROP1("wrong src address");
1036
1037   if (sk->fport != ifa->cf->port)
1038     DROP("wrong src port", sk->fport);
1039
1040   if (len < sizeof(struct babel_pkt_header))
1041     DROP("too short", len);
1042
1043   if (sk->flags & SKF_TRUNCATED)
1044     DROP("truncated", len);
1045
1046   babel_process_packet((struct babel_pkt_header *) sk->rbuf, len, sk->faddr, ifa);
1047   return 1;
1048
1049 drop:
1050   LOG_PKT("Bad packet from %I via %s - %s (%u)",
1051           sk->faddr, sk->iface->name, err_dsc, err_val);
1052   return 1;
1053 }
1054
1055 int
1056 babel_open_socket(struct babel_iface *ifa)
1057 {
1058   struct babel_proto *p = ifa->proto;
1059
1060   sock *sk;
1061   sk = sk_new(ifa->pool);
1062   sk->type = SK_UDP;
1063   sk->sport = ifa->cf->port;
1064   sk->dport = ifa->cf->port;
1065   sk->iface = ifa->iface;
1066
1067   sk->rx_hook = babel_rx_hook;
1068   sk->tx_hook = babel_tx_hook;
1069   sk->err_hook = babel_err_hook;
1070   sk->data = ifa;
1071
1072   sk->tos = ifa->cf->tx_tos;
1073   sk->priority = ifa->cf->tx_priority;
1074   sk->ttl = 1;
1075   sk->flags = SKF_LADDR_RX;
1076
1077   if (sk_open(sk) < 0)
1078     goto err;
1079
1080   if (sk_setup_multicast(sk) < 0)
1081     goto err;
1082
1083   if (sk_join_group(sk, IP6_BABEL_ROUTERS) < 0)
1084     goto err;
1085
1086   ifa->sk = sk;
1087   return 1;
1088
1089 err:
1090   sk_log_error(sk, p->p.name);
1091   rfree(sk);
1092   return 0;
1093 }