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->plen = 0;
 484     msg->prefix = IPA_NONE;
 485     break;
 486
 487   case BABEL_AE_IP4:
 488     /* TODO */
 489     return PARSE_IGNORE;
 490
 491   case BABEL_AE_IP6:
 492     if (tlv->plen > MAX_PREFIX_LENGTH)
 493       return PARSE_ERROR;
 494
 495     /* Cannot omit data if there is no saved prefix */
 496     if (tlv->omitted && !state->def_ip6_prefix_seen)
 497       return PARSE_ERROR;
 498
 499     /* Merge saved prefix and received prefix parts */
 500     memcpy(buf, state->def_ip6_prefix, tlv->omitted);
 501     memcpy(buf + tlv->omitted, tlv->addr, len);
 502
 503     msg->plen = tlv->plen;
 504     msg->prefix = ipa_from_ip6(get_ip6(buf));
 505
 506     if (tlv->flags & BABEL_FLAG_DEF_PREFIX)
 507     {
 508       put_ip6(state->def_ip6_prefix, msg->prefix);
 509       state->def_ip6_prefix_seen = 1;
 510     }
 511
 512     if (tlv->flags & BABEL_FLAG_ROUTER_ID)
 513     {
 514       state->router_id = ((u64) _I2(msg->prefix)) << 32 | _I3(msg->prefix);
 515       state->router_id_seen = 1;
 516     }
 517     break;
 518
 519   case BABEL_AE_IP6_LL:
 520     /* ??? */
 521     return PARSE_IGNORE;
 522
 523   default:
 524     return PARSE_IGNORE;
 525   }
 526
 527   /* Update must have Router ID, unless it is retraction */
 528   if (!state->router_id_seen && (msg->metric != BABEL_INFINITY))
 529   {
 530     DBG("Babel: No router ID seen before update\n");
 531     return PARSE_ERROR;
 532   }
 533
 534   msg->router_id = state->router_id;
 535   msg->next_hop = state->next_hop;
 536   msg->sender = state->saddr;
 537
 538   return PARSE_SUCCESS;
 539 }
 540
 541 static int
 542 babel_write_update(struct babel_tlv *hdr, union babel_msg *m,
 543                    struct babel_write_state *state, int max_len)
 544 {
 545   struct babel_tlv_update *tlv = (void *) hdr;
 546   struct babel_msg_update *msg = &m->update;
 547   int len0 = 0;
 548
 549   /*
 550    * When needed, we write Router-ID TLV before Update TLV and return size of
 551    * both of them. There is enough space for the Router-ID TLV, because
 552    * sizeof(struct babel_tlv_router_id) == sizeof(struct babel_tlv_update).
 553    */
 554   if (!state->router_id_seen || (msg->router_id != state->router_id))
 555   {
 556     len0 = babel_write_router_id(hdr, msg->router_id, state, max_len);
 557     tlv = (struct babel_tlv_update *) NEXT_TLV(tlv);
 558   }
 559
 560   int len = sizeof(struct babel_tlv_update) + (msg->plen + 7)/8;
 561
 562   if (len0 + len > max_len)
 563     return 0;
 564
 565   memset(tlv, 0, sizeof(struct babel_tlv_update));
 566   TLV_HDR(tlv, BABEL_TLV_UPDATE, len);
 567   tlv->ae = BABEL_AE_IP6;
 568   tlv->plen = msg->plen;
 569   put_time16(&tlv->interval, msg->interval);
 570   put_u16(&tlv->seqno, msg->seqno);
 571   put_u16(&tlv->metric, msg->metric);
 572   put_ip6_px(tlv->addr, msg->prefix, msg->plen);
 573
 574   return len0 + len;
 575 }
 576
 577 static int
 578 babel_read_route_request(struct babel_tlv *hdr, union babel_msg *m,
 579                          struct babel_parse_state *state)
 580 {
 581   struct babel_tlv_route_request *tlv = (void *) hdr;
 582   struct babel_msg_route_request *msg = &m->route_request;
 583
 584   msg->type = BABEL_TLV_ROUTE_REQUEST;
 585
 586   switch (tlv->ae)
 587   {
 588   case BABEL_AE_WILDCARD:
 589     /* Wildcard requests must have plen 0 */
 590     if (tlv->plen > 0)
 591       return PARSE_ERROR;
 592
 593     msg->full = 1;
 594     return PARSE_SUCCESS;
 595
 596   case BABEL_AE_IP4:
 597     /* TODO */
 598     return PARSE_IGNORE;
 599
 600   case BABEL_AE_IP6:
 601     if (tlv->plen > MAX_PREFIX_LENGTH)
 602       return PARSE_ERROR;
 603
 604     if (TLV_OPT_LENGTH(tlv) < (tlv->plen + 7)/8)
 605       return PARSE_ERROR;
 606
 607     msg->plen = tlv->plen;
 608     msg->prefix = get_ip6_px(tlv->addr, tlv->plen);
 609     return PARSE_SUCCESS;
 610
 611   case BABEL_AE_IP6_LL:
 612     return PARSE_ERROR;
 613
 614   default:
 615     return PARSE_IGNORE;
 616   }
 617
 618   return PARSE_IGNORE;
 619 }
 620
 621 static int
 622 babel_write_route_request(struct babel_tlv *hdr, union babel_msg *m,
 623                           struct babel_write_state *state, int max_len)
 624 {
 625   struct babel_tlv_route_request *tlv = (void *) hdr;
 626   struct babel_msg_route_request *msg = &m->route_request;
 627
 628   int len = sizeof(struct babel_tlv_route_request) + (msg->plen + 7)/8;
 629
 630   if (len > max_len)
 631     return 0;
 632
 633   TLV_HDR(tlv, BABEL_TLV_ROUTE_REQUEST, len);
 634
 635   if (msg->full)
 636   {
 637     tlv->ae = BABEL_AE_WILDCARD;
 638     tlv->plen = 0;
 639   }
 640   else
 641   {
 642     tlv->ae = BABEL_AE_IP6;
 643     tlv->plen = msg->plen;
 644     put_ip6_px(tlv->addr, msg->prefix, msg->plen);
 645   }
 646
 647   return len;
 648 }
 649
 650 static int
 651 babel_read_seqno_request(struct babel_tlv *hdr, union babel_msg *m,
 652                          struct babel_parse_state *state)
 653 {
 654   struct babel_tlv_seqno_request *tlv = (void *) hdr;
 655   struct babel_msg_seqno_request *msg = &m->seqno_request;
 656
 657   msg->type = BABEL_TLV_SEQNO_REQUEST;
 658   msg->seqno = get_u16(&tlv->seqno);
 659   msg->hop_count = tlv->hop_count;
 660   msg->router_id = get_u64(&tlv->router_id);
 661   msg->sender = state->saddr;
 662
 663   if (tlv->hop_count == 0)
 664     return PARSE_ERROR;
 665
 666   switch (tlv->ae)
 667   {
 668   case BABEL_AE_WILDCARD:
 669     return PARSE_ERROR;
 670
 671   case BABEL_AE_IP4:
 672     /* TODO */
 673     return PARSE_IGNORE;
 674
 675   case BABEL_AE_IP6:
 676     if (tlv->plen > MAX_PREFIX_LENGTH)
 677       return PARSE_ERROR;
 678
 679     if (TLV_OPT_LENGTH(tlv) < (tlv->plen + 7)/8)
 680       return PARSE_ERROR;
 681
 682     msg->plen = tlv->plen;
 683     msg->prefix = get_ip6_px(tlv->addr, tlv->plen);
 684     return PARSE_SUCCESS;
 685
 686   case BABEL_AE_IP6_LL:
 687     return PARSE_ERROR;
 688
 689   default:
 690     return PARSE_IGNORE;
 691   }
 692
 693   return PARSE_IGNORE;
 694 }
 695
 696 static int
 697 babel_write_seqno_request(struct babel_tlv *hdr, union babel_msg *m,
 698                           struct babel_write_state *state, int max_len)
 699 {
 700   struct babel_tlv_seqno_request *tlv = (void *) hdr;
 701   struct babel_msg_seqno_request *msg = &m->seqno_request;
 702
 703   int len = sizeof(struct babel_tlv_seqno_request) + (msg->plen + 7)/8;
 704
 705   if (len > max_len)
 706     return 0;
 707
 708   TLV_HDR(tlv, BABEL_TLV_SEQNO_REQUEST, len);
 709   tlv->ae = BABEL_AE_IP6;
 710   tlv->plen = msg->plen;
 711   put_u16(&tlv->seqno, msg->seqno);
 712   tlv->hop_count = msg->hop_count;
 713   put_u64(&tlv->router_id, msg->router_id);
 714   put_ip6_px(tlv->addr, msg->prefix, msg->plen);
 715
 716   return len;
 717 }
 718
 719 static inline int
 720 babel_read_tlv(struct babel_tlv *hdr,
 721                union babel_msg *msg,
 722                struct babel_parse_state *state)
 723 {
 724   if ((hdr->type <= BABEL_TLV_PADN) ||
 725       (hdr->type >= BABEL_TLV_MAX) ||
 726       !tlv_data[hdr->type].read_tlv)
 727     return PARSE_IGNORE;
 728
 729   if (TLV_LENGTH(hdr) < tlv_data[hdr->type].min_length)
 730     return PARSE_ERROR;
 731
 732   memset(msg, 0, sizeof(*msg));
 733   return tlv_data[hdr->type].read_tlv(hdr, msg, state);
 734 }
 735
 736 static int
 737 babel_write_tlv(struct babel_tlv *hdr,
 738                 union babel_msg *msg,
 739                 struct babel_write_state *state,
 740                 int max_len)
 741 {
 742   if ((msg->type <= BABEL_TLV_PADN) ||
 743       (msg->type >= BABEL_TLV_MAX) ||
 744       !tlv_data[msg->type].write_tlv)
 745     return 0;
 746
 747   if (tlv_data[msg->type].min_length > max_len)
 748     return 0;
 749
 750   memset(hdr, 0, tlv_data[msg->type].min_length);
 751   return tlv_data[msg->type].write_tlv(hdr, msg, state, max_len);
 752 }
 753
 754
 755 /*
 756  *      Packet RX/TX functions
 757  */
 758
 759 static int
 760 babel_send_to(struct babel_iface *ifa, ip_addr dest)
 761 {
 762   sock *sk = ifa->sk;
 763   struct babel_pkt_header *hdr = (void *) sk->tbuf;
 764   int len = get_u16(&hdr->length) + sizeof(struct babel_pkt_header);
 765
 766   DBG("Babel: Sending %d bytes to %I\n", len, dest);
 767   return sk_send_to(sk, len, dest, 0);
 768 }
 769
 770 /**
 771  * babel_write_queue - Write a TLV queue to a transmission buffer
 772  * @ifa: Interface holding the transmission buffer
 773  * @queue: TLV queue to write (containing internal-format TLVs)
 774  *
 775  * This function writes a packet to the interface transmission buffer with as
 776  * many TLVs from the &queue as will fit in the buffer. It returns the number of
 777  * bytes written (NOT counting the packet header). The function is called by
 778  * babel_send_queue() and babel_send_unicast() to construct packets for
 779  * transmission, and uses per-TLV helper functions to convert the
 780  * internal-format TLVs to their wire representations.
 781  *
 782  * The TLVs in the queue are freed after they are written to the buffer.
 783  */
 784 static int
 785 babel_write_queue(struct babel_iface *ifa, list *queue)
 786 {
 787   struct babel_proto *p = ifa->proto;
 788   struct babel_write_state state = {};
 789
 790   if (EMPTY_LIST(*queue))
 791     return 0;
 792
 793   byte *pos = ifa->sk->tbuf;
 794   byte *end = pos + ifa->tx_length;
 795
 796   struct babel_pkt_header *pkt = (void *) pos;
 797   pkt->magic = BABEL_MAGIC;
 798   pkt->version = BABEL_VERSION;
 799   pkt->length = 0;
 800   pos += sizeof(struct babel_pkt_header);
 801
 802   struct babel_msg_node *msg;
 803   WALK_LIST_FIRST(msg, *queue)
 804   {
 805     int len = babel_write_tlv((struct babel_tlv *) pos, &msg->msg, &state, end - pos);
 806
 807     if (!len)
 808       break;
 809
 810     pos += len;
 811     rem_node(NODE msg);
 812     sl_free(p->msg_slab, msg);
 813   }
 814
 815   int plen = pos - (byte *) pkt;
 816   put_u16(&pkt->length, plen - sizeof(struct babel_pkt_header));
 817
 818   return plen;
 819 }
 820
 821 void
 822 babel_send_queue(void *arg)
 823 {
 824   struct babel_iface *ifa = arg;
 825   while ((babel_write_queue(ifa, &ifa->msg_queue) > 0) &&
 826          (babel_send_to(ifa, IP6_BABEL_ROUTERS) > 0));
 827 }
 828
 829 static inline void
 830 babel_kick_queue(struct babel_iface *ifa)
 831 {
 832   /*
 833    * Only schedule send event if there is not already data in the socket buffer.
 834    * Otherwise we may overwrite the data already in the buffer.
 835    */
 836
 837   if ((ifa->sk->tpos == ifa->sk->tbuf) && !ev_active(ifa->send_event))
 838     ev_schedule(ifa->send_event);
 839 }
 840
 841 /**
 842  * babel_send_unicast - send a single TLV via unicast to a destination
 843  * @msg: TLV to send
 844  * @ifa: Interface to send via
 845  * @dest: Destination of the TLV
 846  *
 847  * This function is used to send a single TLV via unicast to a designated
 848  * receiver. This is used for replying to certain incoming requests, and for
 849  * sending unicast requests to refresh routes before they expire.
 850  */
 851 void
 852 babel_send_unicast(union babel_msg *msg, struct babel_iface *ifa, ip_addr dest)
 853 {
 854   struct babel_proto *p = ifa->proto;
 855   struct babel_msg_node *msgn = sl_alloc(p->msg_slab);
 856   list queue;
 857
 858   msgn->msg = *msg;
 859   init_list(&queue);
 860   add_tail(&queue, NODE msgn);
 861   babel_write_queue(ifa, &queue);
 862   babel_send_to(ifa, dest);
 863
 864   /* We could overwrite waiting packet here, we may have to kick TX queue */
 865   if (!EMPTY_LIST(ifa->msg_queue))
 866     babel_kick_queue(ifa);
 867 }
 868
 869 /**
 870  * babel_enqueue - enqueue a TLV for transmission on an interface
 871  * @msg: TLV to enqueue (in internal TLV format)
 872  * @ifa: Interface to enqueue to
 873  *
 874  * This function is called to enqueue a TLV for subsequent transmission on an
 875  * interface. The transmission event is triggered whenever a TLV is enqueued;
 876  * this ensures that TLVs will be transmitted in a timely manner, but that TLVs
 877  * which are enqueued in rapid succession can be transmitted together in one
 878  * packet.
 879  */
 880 void
 881 babel_enqueue(union babel_msg *msg, struct babel_iface *ifa)
 882 {
 883   struct babel_proto *p = ifa->proto;
 884   struct babel_msg_node *msgn = sl_alloc(p->msg_slab);
 885   msgn->msg = *msg;
 886   add_tail(&ifa->msg_queue, NODE msgn);
 887   babel_kick_queue(ifa);
 888 }
 889
 890 /**
 891  * babel_process_packet - process incoming data packet
 892  * @pkt: Pointer to the packet data
 893  * @len: Length of received packet
 894  * @saddr: Address of packet sender
 895  * @ifa: Interface packet was received on.
 896  *
 897  * This function is the main processing hook of incoming Babel packets. It
 898  * checks that the packet header is well-formed, then processes the TLVs
 899  * contained in the packet. This is done in two passes: First all TLVs are
 900  * parsed into the internal TLV format. If a TLV parser fails, processing of the
 901  * rest of the packet is aborted.
 902  *
 903  * After the parsing step, the TLV handlers are called for each parsed TLV in
 904  * order.
 905  */
 906 static void
 907 babel_process_packet(struct babel_pkt_header *pkt, int len,
 908                      ip_addr saddr, struct babel_iface *ifa)
 909 {
 910   struct babel_proto *p = ifa->proto;
 911   struct babel_tlv *tlv;
 912   struct babel_msg_node *msg;
 913   list msgs;
 914   int res;
 915
 916   int plen = sizeof(struct babel_pkt_header) + get_u16(&pkt->length);
 917   byte *pos;
 918   byte *end = (byte *)pkt + plen;
 919
 920   struct babel_parse_state state = {
 921     .proto      = p,
 922     .ifa        = ifa,
 923     .saddr      = saddr,
 924     .next_hop   = saddr,
 925   };
 926
 927   if ((pkt->magic != BABEL_MAGIC) || (pkt->version != BABEL_VERSION))
 928   {
 929     TRACE(D_PACKETS, "Strange packet from %I via %s - magic %d version %d",
 930           saddr, ifa->iface->name, pkt->magic, pkt->version);
 931     return;
 932   }
 933
 934   if (plen > len)
 935   {
 936     LOG_PKT("Bad packet from %I via %s - %s (%u)",
 937             saddr, ifa->iface->name, "length mismatch", plen);
 938     return;
 939   }
 940
 941   TRACE(D_PACKETS, "Packet received from %I via %s",
 942         saddr, ifa->iface->name);
 943
 944   init_list(&msgs);
 945
 946   /* First pass through the packet TLV by TLV, parsing each into internal data
 947      structures. */
 948   for (tlv = FIRST_TLV(pkt);
 949        (byte *)tlv < end;
 950        tlv = NEXT_TLV(tlv))
 951   {
 952     /* Ugly special case */
 953     if (tlv->type == BABEL_TLV_PAD1)
 954       continue;
 955
 956     /* The end of the common TLV header */
 957     pos = (byte *)tlv + sizeof(struct babel_tlv);
 958     if ((pos > end) || (pos + tlv->length > end))
 959     {
 960       LOG_PKT("Bad TLV from %I via %s type %d pos %d - framing error",
 961               saddr, ifa->iface->name, tlv->type, (byte *)tlv - (byte *)pkt);
 962       break;
 963     }
 964
 965     msg = sl_alloc(p->msg_slab);
 966     res = babel_read_tlv(tlv, &msg->msg, &state);
 967     if (res == PARSE_SUCCESS)
 968     {
 969       add_tail(&msgs, NODE msg);
 970     }
 971     else if (res == PARSE_IGNORE)
 972     {
 973       DBG("Babel: Ignoring TLV of type %d\n", tlv->type);
 974       sl_free(p->msg_slab, msg);
 975     }
 976     else /* PARSE_ERROR */
 977     {
 978       LOG_PKT("Bad TLV from %I via %s type %d pos %d - parse error",
 979               saddr, ifa->iface->name, tlv->type, (byte *)tlv - (byte *)pkt);
 980       sl_free(p->msg_slab, msg);
 981       break;
 982     }
 983   }
 984
 985   /* Parsing done, handle all parsed TLVs */
 986   WALK_LIST_FIRST(msg, msgs)
 987   {
 988     if (tlv_data[msg->msg.type].handle_tlv)
 989       tlv_data[msg->msg.type].handle_tlv(&msg->msg, ifa);
 990     rem_node(NODE msg);
 991     sl_free(p->msg_slab, msg);
 992   }
 993 }
 994
 995 static void
 996 babel_err_hook(sock *sk, int err)
 997 {
 998   struct babel_iface *ifa = sk->data;
 999   struct babel_proto *p = ifa->proto;
1000
1001   log(L_ERR "%s: Socket error on %s: %M", p->p.name, ifa->iface->name, err);
1002   /* FIXME: Drop queued TLVs here? */
1003 }
1004
1005
1006 static void
1007 babel_tx_hook(sock *sk)
1008 {
1009   struct babel_iface *ifa = sk->data;
1010
1011   DBG("Babel: TX hook called (iface %s, src %I, dst %I)\n",
1012       sk->iface->name, sk->saddr, sk->daddr);
1013
1014   babel_send_queue(ifa);
1015 }
1016
1017
1018 static int
1019 babel_rx_hook(sock *sk, int len)
1020 {
1021   struct babel_iface *ifa = sk->data;
1022   struct babel_proto *p = ifa->proto;
1023   const char *err_dsc = NULL;
1024   uint err_val = 0;
1025
1026   if (sk->lifindex != ifa->iface->index)
1027     return 1;
1028
1029   DBG("Babel: RX hook called (iface %s, src %I, dst %I)\n",
1030       sk->iface->name, sk->faddr, sk->laddr);
1031
1032   /* Silently ignore my own packets */
1033   if (ipa_equal(ifa->iface->addr->ip, sk->faddr))
1034     return 1;
1035
1036   if (!ipa_is_link_local(sk->faddr))
1037     DROP1("wrong src address");
1038
1039   if (sk->fport != ifa->cf->port)
1040     DROP("wrong src port", sk->fport);
1041
1042   if (len < sizeof(struct babel_pkt_header))
1043     DROP("too short", len);
1044
1045   if (sk->flags & SKF_TRUNCATED)
1046     DROP("truncated", len);
1047
1048   babel_process_packet((struct babel_pkt_header *) sk->rbuf, len, sk->faddr, ifa);
1049   return 1;
1050
1051 drop:
1052   LOG_PKT("Bad packet from %I via %s - %s (%u)",
1053           sk->faddr, sk->iface->name, err_dsc, err_val);
1054   return 1;
1055 }
1056
1057 int
1058 babel_open_socket(struct babel_iface *ifa)
1059 {
1060   struct babel_proto *p = ifa->proto;
1061
1062   sock *sk;
1063   sk = sk_new(ifa->pool);
1064   sk->type = SK_UDP;
1065   sk->sport = ifa->cf->port;
1066   sk->dport = ifa->cf->port;
1067   sk->iface = ifa->iface;
1068
1069   sk->rx_hook = babel_rx_hook;
1070   sk->tx_hook = babel_tx_hook;
1071   sk->err_hook = babel_err_hook;
1072   sk->data = ifa;
1073
1074   sk->tos = ifa->cf->tx_tos;
1075   sk->priority = ifa->cf->tx_priority;
1076   sk->ttl = 1;
1077   sk->flags = SKF_LADDR_RX;
1078
1079   if (sk_open(sk) < 0)
1080     goto err;
1081
1082   if (sk_setup_multicast(sk) < 0)
1083     goto err;
1084
1085   if (sk_join_group(sk, IP6_BABEL_ROUTERS) < 0)
1086     goto err;
1087
1088   ifa->sk = sk;
1089   return 1;
1090
1091 err:
1092   sk_log_error(sk, p->p.name);
1093   rfree(sk);
1094   return 0;
1095 }