/proto/babel/packets.c - Annotate - Internet on FIRE - Redmine

iof-bird-daemon / proto / babel / packets.c @ 8b58f565

History | View | Annotate | Download (33.6 KB)

-e75d8
+/*
  *        BIRD -- The Babel protocol
+ *
  *        Copyright (c) 2015--2016 Toke Hoiland-Jorgensen
+ *
  *        Can be freely distributed and used under the terms of the GNU GPL.
+ *
  *        This file contains the packet and TLV handling code for the protocol.
  */
 #include "babel.h"
 struct babel_pkt_header {
   u8 magic;
   u8 version;
   u16 length;
 } PACKED;
 struct babel_tlv {
   u8 type;
   u8 length;
   u8 value[0];
 } PACKED;
 struct babel_tlv_ack_req {
   u8 type;
   u8 length;
   u16 reserved;
   u16 nonce;
   u16 interval;
 } PACKED;
 struct babel_tlv_ack {
   u8 type;
   u8 length;
   u16 nonce;
 } PACKED;
 struct babel_tlv_hello {
   u8 type;
   u8 length;
   u16 reserved;
   u16 seqno;
   u16 interval;
 } PACKED;
 struct babel_tlv_ihu {
   u8 type;
   u8 length;
   u8 ae;
   u8 reserved;
   u16 rxcost;
   u16 interval;
   u8 addr[0];
 } PACKED;
 struct babel_tlv_router_id {
   u8 type;
   u8 length;
   u16 reserved;
   u64 router_id;
 } PACKED;
 struct babel_tlv_next_hop {
   u8 type;
   u8 length;
   u8 ae;
   u8 reserved;
   u8 addr[0];
 } PACKED;
 struct babel_tlv_update {
   u8 type;
   u8 length;
   u8 ae;
   u8 flags;
   u8 plen;
   u8 omitted;
   u16 interval;
   u16 seqno;
   u16 metric;
   u8 addr[0];
 } PACKED;
 struct babel_tlv_route_request {
   u8 type;
   u8 length;
   u8 ae;
   u8 plen;
   u8 addr[0];
 } PACKED;
 struct babel_tlv_seqno_request {
   u8 type;
   u8 length;
   u8 ae;
   u8 plen;
   u16 seqno;
   u8 hop_count;
   u8 reserved;
   u64 router_id;
   u8 addr[0];
 } PACKED;
 #define BABEL_FLAG_DEF_PREFIX        0x80
 #define BABEL_FLAG_ROUTER_ID        0x40
 struct babel_parse_state {
   struct babel_proto *proto;
   struct babel_iface *ifa;
   ip_addr saddr;
-            4324025f
+  ip_addr next_hop_ip4;
   ip_addr next_hop_ip6;
-e75d8
+  u64 router_id;                /* Router ID used in subsequent updates */
   u8 def_ip6_prefix[16];        /* Implicit IPv6 prefix in network order */
   u8 def_ip4_prefix[4];                /* Implicit IPv4 prefix in network order */
   u8 router_id_seen;                /* router_id field is valid */
   u8 def_ip6_prefix_seen;        /* def_ip6_prefix is valid */
   u8 def_ip4_prefix_seen;        /* def_ip4_prefix is valid */
-ebfa1
+  u8 current_tlv_endpos;        /* End of self-terminating TLVs (offset from start) */
-e75d8
+};
 enum parse_result {
   PARSE_SUCCESS,
   PARSE_ERROR,
   PARSE_IGNORE,
 };
 struct babel_write_state {
   u64 router_id;
   u8 router_id_seen;
-            4324025f
+  ip_addr next_hop_ip4;
   ip_addr next_hop_ip6;
-            b3c6273e
+  u8 def_ip6_prefix[16];        /* Implicit IPv6 prefix in network order */
   u8 def_ip6_pxlen;
-e75d8
+};
 #define DROP(DSC,VAL) do { err_dsc = DSC; err_val = VAL; goto drop; } while(0)
 #define DROP1(DSC) do { err_dsc = DSC; goto drop; } while(0)
 #define LOG_PKT(msg, args...) \
   log_rl(&p->log_pkt_tbf, L_REMOTE "%s: " msg, p->p.name, args)
 #define FIRST_TLV(p) ((struct babel_tlv *) (((struct babel_pkt_header *) p) + 1))
 #define NEXT_TLV(t) ((struct babel_tlv *) (((byte *) t) + TLV_LENGTH(t)))
 #define TLV_LENGTH(t) (t->type == BABEL_TLV_PAD1 ? 1 : t->length + sizeof(struct babel_tlv))
 #define TLV_OPT_LENGTH(t) (t->length + sizeof(struct babel_tlv) - sizeof(*t))
 #define TLV_HDR(tlv,t,l) ({ tlv->type = t; tlv->length = l - sizeof(struct babel_tlv); })
 #define TLV_HDR0(tlv,t) TLV_HDR(tlv, t, tlv_data[t].min_length)
-e8df049
+#define NET_SIZE(n) BYTES(net_pxlen(n))
-            b3c6273e
+static inline uint
 bytes_equal(u8 *b1, u8 *b2, uint maxlen)
+{
   uint i;
   for (i = 0; (i < maxlen) && (*b1 == *b2); i++, b1++, b2++)
+    ;
   return i;
+}
-e75d8
+            Ondrej Zajicek (work)
-b58f565
+static inline uint
-e75d8
+get_time16(const void *p)
+{
-b58f565
+  uint v = get_u16(p) * BABEL_TIME_UNITS;
   return MAX(BABEL_MIN_INTERVAL, v);
-e75d8
+            Ondrej Zajicek (work)
 static inline void
-b58f565
+put_time16(void *p, uint v)
-e75d8
+            Ondrej Zajicek (work)
-b58f565
+  put_u16(p, v / BABEL_TIME_UNITS);
-e75d8
+            Ondrej Zajicek (work)
-e8df049
+static inline void
-            4324025f
+read_ip4_px(net_addr *n, const void *p, uint plen)
+{
   ip4_addr addr = {0};
   memcpy(&addr, p, BYTES(plen));
   net_fill_ip4(n, ip4_ntoh(addr), plen);
+}
 static inline void
 put_ip4_px(void *p, net_addr *n)
+{
   ip4_addr addr = ip4_hton(net4_prefix(n));
   memcpy(p, &addr, NET_SIZE(n));
+}
 static inline void
-e8df049
+read_ip6_px(net_addr *n, const void *p, uint plen)
-e75d8
+            Ondrej Zajicek (work)
   ip6_addr addr = IPA_NONE;
-e236955
+  memcpy(&addr, p, BYTES(plen));
-e8df049
+  net_fill_ip6(n, ip6_ntoh(addr), plen);
-e75d8
+            Ondrej Zajicek (work)
 static inline void
-e8df049
+put_ip6_px(void *p, net_addr *n)
-e75d8
+            Ondrej Zajicek (work)
-e8df049
+  ip6_addr addr = ip6_hton(net6_prefix(n));
   memcpy(p, &addr, NET_SIZE(n));
-e75d8
+            Ondrej Zajicek (work)
 static inline ip6_addr
 get_ip6_ll(const void *p)
+{
   return ip6_build(0xfe800000, 0, get_u32(p+0), get_u32(p+4));
+}
 static inline void
 put_ip6_ll(void *p, ip6_addr addr)
+{
   put_u32(p+0, _I2(addr));
   put_u32(p+4, _I3(addr));
+}
 /*
  *        TLV read/write functions
  */
 static int babel_read_ack_req(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
 static int babel_read_hello(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
 static int babel_read_ihu(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
 static int babel_read_router_id(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
 static int babel_read_next_hop(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
 static int babel_read_update(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
 static int babel_read_route_request(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
 static int babel_read_seqno_request(struct babel_tlv *hdr, union babel_msg *msg, struct babel_parse_state *state);
-e236955
+static uint babel_write_ack(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, uint max_len);
 static uint babel_write_hello(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, uint max_len);
 static uint babel_write_ihu(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, uint max_len);
 static uint babel_write_update(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, uint max_len);
 static uint babel_write_route_request(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, uint max_len);
 static uint babel_write_seqno_request(struct babel_tlv *hdr, union babel_msg *msg, struct babel_write_state *state, uint max_len);
-e75d8
+            Ondrej Zajicek (work)
 struct babel_tlv_data {
   u8 min_length;
   int (*read_tlv)(struct babel_tlv *hdr, union babel_msg *m, struct babel_parse_state *state);
-e236955
+  uint (*write_tlv)(struct babel_tlv *hdr, union babel_msg *m, struct babel_write_state *state, uint max_len);
-e75d8
+  void (*handle_tlv)(union babel_msg *m, struct babel_iface *ifa);
 };
-e236955
+static const struct babel_tlv_data tlv_data[BABEL_TLV_MAX] = {
-e75d8
+  [BABEL_TLV_ACK_REQ] = {
     sizeof(struct babel_tlv_ack_req),
     babel_read_ack_req,
     NULL,
     babel_handle_ack_req
   },
   [BABEL_TLV_ACK] = {
     sizeof(struct babel_tlv_ack),
     NULL,
     babel_write_ack,
     NULL
   },
   [BABEL_TLV_HELLO] = {
     sizeof(struct babel_tlv_hello),
     babel_read_hello,
     babel_write_hello,
     babel_handle_hello
   },
   [BABEL_TLV_IHU] = {
     sizeof(struct babel_tlv_ihu),
     babel_read_ihu,
     babel_write_ihu,
     babel_handle_ihu
   },
   [BABEL_TLV_ROUTER_ID] = {
     sizeof(struct babel_tlv_router_id),
     babel_read_router_id,
     NULL,
     NULL
   },
   [BABEL_TLV_NEXT_HOP] = {
     sizeof(struct babel_tlv_next_hop),
     babel_read_next_hop,
     NULL,
     NULL
   },
   [BABEL_TLV_UPDATE] = {
     sizeof(struct babel_tlv_update),
     babel_read_update,
     babel_write_update,
     babel_handle_update
   },
   [BABEL_TLV_ROUTE_REQUEST] = {
     sizeof(struct babel_tlv_route_request),
     babel_read_route_request,
     babel_write_route_request,
     babel_handle_route_request
   },
   [BABEL_TLV_SEQNO_REQUEST] = {
     sizeof(struct babel_tlv_seqno_request),
     babel_read_seqno_request,
     babel_write_seqno_request,
     babel_handle_seqno_request
   },
 };
 static int
 babel_read_ack_req(struct babel_tlv *hdr, union babel_msg *m,
                    struct babel_parse_state *state)
+{
   struct babel_tlv_ack_req *tlv = (void *) hdr;
   struct babel_msg_ack_req *msg = &m->ack_req;
   msg->type = BABEL_TLV_ACK_REQ;
   msg->nonce = get_u16(&tlv->nonce);
   msg->interval = get_time16(&tlv->interval);
   msg->sender = state->saddr;
   if (!msg->interval)
     return PARSE_ERROR;
   return PARSE_SUCCESS;
+}
-e236955
+static uint
-e75d8
+babel_write_ack(struct babel_tlv *hdr, union babel_msg *m,
-e236955
+                struct babel_write_state *state UNUSED, uint max_len UNUSED)
-e75d8
+            Ondrej Zajicek (work)
   struct babel_tlv_ack *tlv = (void *) hdr;
   struct babel_msg_ack *msg = &m->ack;
   TLV_HDR0(tlv, BABEL_TLV_ACK);
   put_u16(&tlv->nonce, msg->nonce);
   return sizeof(struct babel_tlv_ack);
+}
 static int
 babel_read_hello(struct babel_tlv *hdr, union babel_msg *m,
                  struct babel_parse_state *state)
+{
   struct babel_tlv_hello *tlv = (void *) hdr;
   struct babel_msg_hello *msg = &m->hello;
   msg->type = BABEL_TLV_HELLO;
   msg->seqno = get_u16(&tlv->seqno);
   msg->interval = get_time16(&tlv->interval);
   msg->sender = state->saddr;
   return PARSE_SUCCESS;
+}
-e236955
+static uint
-e75d8
+babel_write_hello(struct babel_tlv *hdr, union babel_msg *m,
-e236955
+                  struct babel_write_state *state UNUSED, uint max_len UNUSED)
-e75d8
+            Ondrej Zajicek (work)
   struct babel_tlv_hello *tlv = (void *) hdr;
   struct babel_msg_hello *msg = &m->hello;
   TLV_HDR0(tlv, BABEL_TLV_HELLO);
   put_u16(&tlv->seqno, msg->seqno);
   put_time16(&tlv->interval, msg->interval);
   return sizeof(struct babel_tlv_hello);
+}
 static int
 babel_read_ihu(struct babel_tlv *hdr, union babel_msg *m,
                struct babel_parse_state *state)
+{
   struct babel_tlv_ihu *tlv = (void *) hdr;
   struct babel_msg_ihu *msg = &m->ihu;
   msg->type = BABEL_TLV_IHU;
   msg->ae = tlv->ae;
   msg->rxcost = get_u16(&tlv->rxcost);
   msg->interval = get_time16(&tlv->interval);
   msg->addr = IPA_NONE;
   msg->sender = state->saddr;
   if (msg->ae >= BABEL_AE_MAX)
     return PARSE_IGNORE;
-ebfa1
+  /*
    * We only actually read link-local IPs. In every other case, the addr field
    * will be 0 but validation will succeed. The handler takes care of these
    * cases. We handle them here anyway because we need the length for parsing
    * subtlvs.
    */
   switch (msg->ae)
-e75d8
+            Ondrej Zajicek (work)
-ebfa1
+  case BABEL_AE_IP4:
     if (TLV_OPT_LENGTH(tlv) < 4)
       return PARSE_ERROR;
     state->current_tlv_endpos += 4;
     break;
   case BABEL_AE_IP6:
     if (TLV_OPT_LENGTH(tlv) < 16)
       return PARSE_ERROR;
     state->current_tlv_endpos += 16;
     break;
   case BABEL_AE_IP6_LL:
-e75d8
+    if (TLV_OPT_LENGTH(tlv) < 8)
       return PARSE_ERROR;
     msg->addr = ipa_from_ip6(get_ip6_ll(&tlv->addr));
-ebfa1
+    state->current_tlv_endpos += 8;
     break;
-e75d8
+            Ondrej Zajicek (work)
   return PARSE_SUCCESS;
+}
-e236955
+static uint
-e75d8
+babel_write_ihu(struct babel_tlv *hdr, union babel_msg *m,
-e236955
+                struct babel_write_state *state UNUSED, uint max_len)
-e75d8
+            Ondrej Zajicek (work)
   struct babel_tlv_ihu *tlv = (void *) hdr;
   struct babel_msg_ihu *msg = &m->ihu;
   if (ipa_is_link_local(msg->addr) && max_len < sizeof(struct babel_tlv_ihu) + 8)
     return 0;
   TLV_HDR0(tlv, BABEL_TLV_IHU);
   put_u16(&tlv->rxcost, msg->rxcost);
   put_time16(&tlv->interval, msg->interval);
   if (!ipa_is_link_local(msg->addr))
+  {
     tlv->ae = BABEL_AE_WILDCARD;
     return sizeof(struct babel_tlv_ihu);
+  }
   put_ip6_ll(&tlv->addr, msg->addr);
   tlv->ae = BABEL_AE_IP6_LL;
   hdr->length += 8;
   return sizeof(struct babel_tlv_ihu) + 8;
+}
 static int
 babel_read_router_id(struct babel_tlv *hdr, union babel_msg *m UNUSED,
                      struct babel_parse_state *state)
+{
   struct babel_tlv_router_id *tlv = (void *) hdr;
   state->router_id = get_u64(&tlv->router_id);
   state->router_id_seen = 1;
   return PARSE_IGNORE;
+}
 /* This is called directly from babel_write_update() */
-e236955
+static uint
-e75d8
+babel_write_router_id(struct babel_tlv *hdr, u64 router_id,
-e236955
+                      struct babel_write_state *state, uint max_len UNUSED)
-e75d8
+            Ondrej Zajicek (work)
   struct babel_tlv_router_id *tlv = (void *) hdr;
   /* We still assume that first min_length bytes are available and zeroed */
   TLV_HDR0(tlv, BABEL_TLV_ROUTER_ID);
   put_u64(&tlv->router_id, router_id);
   state->router_id = router_id;
   state->router_id_seen = 1;
   return sizeof(struct babel_tlv_router_id);
+}
 static int
 babel_read_next_hop(struct babel_tlv *hdr, union babel_msg *m UNUSED,
                     struct babel_parse_state *state)
+{
   struct babel_tlv_next_hop *tlv = (void *) hdr;
   switch (tlv->ae)
+  {
   case BABEL_AE_WILDCARD:
     return PARSE_ERROR;
   case BABEL_AE_IP4:
-            4324025f
+    if (TLV_OPT_LENGTH(tlv) < sizeof(ip4_addr))
       return PARSE_ERROR;
     state->next_hop_ip4 = ipa_from_ip4(get_ip4(&tlv->addr));
-ebfa1
+    state->current_tlv_endpos += sizeof(ip4_addr);
-e75d8
+    return PARSE_IGNORE;
   case BABEL_AE_IP6:
     if (TLV_OPT_LENGTH(tlv) < sizeof(ip6_addr))
       return PARSE_ERROR;
-            4324025f
+    state->next_hop_ip6 = ipa_from_ip6(get_ip6(&tlv->addr));
-ebfa1
+    state->current_tlv_endpos += sizeof(ip6_addr);
-e75d8
+    return PARSE_IGNORE;
   case BABEL_AE_IP6_LL:
     if (TLV_OPT_LENGTH(tlv) < 8)
       return PARSE_ERROR;
-            4324025f
+    state->next_hop_ip6 = ipa_from_ip6(get_ip6_ll(&tlv->addr));
-ebfa1
+    state->current_tlv_endpos += 8;
-e75d8
+    return PARSE_IGNORE;
   default:
     return PARSE_IGNORE;
+  }
   return PARSE_IGNORE;
+}
-            4324025f
+/* This is called directly from babel_write_update() and returns -1 if a next
    hop should be written but there is not enough space. */
 static int
 babel_write_next_hop(struct babel_tlv *hdr, ip_addr addr,
                      struct babel_write_state *state, uint max_len)
+{
   struct babel_tlv_next_hop *tlv = (void *) hdr;
   if (ipa_zero(addr))
+  {
     /* Should not happen */
     return 0;
+  }
   else if (ipa_is_ip4(addr) && !ipa_equal(addr, state->next_hop_ip4))
+  {
     uint len = sizeof(struct babel_tlv_next_hop) + sizeof(ip4_addr);
     if (len > max_len)
       return -1;
     TLV_HDR(tlv, BABEL_TLV_NEXT_HOP, len);
     tlv->ae = BABEL_AE_IP4;
     put_ip4(&tlv->addr, ipa_to_ip4(addr));
     state->next_hop_ip4 = addr;
     return len;
+  }
   else if (ipa_is_ip6(addr) && !ipa_equal(addr, state->next_hop_ip6))
+  {
     uint len = sizeof(struct babel_tlv_next_hop) + sizeof(ip6_addr);
     if (len > max_len)
       return -1;
     TLV_HDR(tlv, BABEL_TLV_NEXT_HOP, len);
     tlv->ae = BABEL_AE_IP6;
     put_ip6(&tlv->addr, ipa_to_ip6(addr));
     state->next_hop_ip6 = addr;
     return len;
+  }
   return 0;
+}
-e75d8
+static int
 babel_read_update(struct babel_tlv *hdr, union babel_msg *m,
                   struct babel_parse_state *state)
+{
   struct babel_tlv_update *tlv = (void *) hdr;
   struct babel_msg_update *msg = &m->update;
   msg->type = BABEL_TLV_UPDATE;
   msg->interval = get_time16(&tlv->interval);
   msg->seqno = get_u16(&tlv->seqno);
   msg->metric = get_u16(&tlv->metric);
   /* Length of received prefix data without omitted part */
-e236955
+  int len = BYTES(tlv->plen) - (int) tlv->omitted;
-e75d8
+  u8 buf[16] = {};
-e236955
+  if ((len < 0) || ((uint) len > TLV_OPT_LENGTH(tlv)))
-e75d8
+    return PARSE_ERROR;
   switch (tlv->ae)
+  {
   case BABEL_AE_WILDCARD:
     if (tlv->plen > 0)
       return PARSE_ERROR;
-e8df049
+    if (msg->metric != 65535)
       return PARSE_ERROR;
-d6ca220
+    msg->wildcard = 1;
-e75d8
+    break;
   case BABEL_AE_IP4:
-            4324025f
+    if (tlv->plen > IP4_MAX_PREFIX_LENGTH)
       return PARSE_ERROR;
     /* Cannot omit data if there is no saved prefix */
     if (tlv->omitted && !state->def_ip4_prefix_seen)
       return PARSE_ERROR;
     /* Need next hop for v4 routes */
     if (ipa_zero(state->next_hop_ip4))
       return PARSE_ERROR;
     /* Merge saved prefix and received prefix parts */
     memcpy(buf, state->def_ip4_prefix, tlv->omitted);
     memcpy(buf + tlv->omitted, tlv->addr, len);
     ip4_addr prefix4 = get_ip4(buf);
     net_fill_ip4(&msg->net, prefix4, tlv->plen);
     if (tlv->flags & BABEL_FLAG_DEF_PREFIX)
+    {
       put_ip4(state->def_ip4_prefix, prefix4);
       state->def_ip4_prefix_seen = 1;
+    }
     msg->next_hop = state->next_hop_ip4;
     break;
-e75d8
+            Ondrej Zajicek (work)
   case BABEL_AE_IP6:
-e8df049
+    if (tlv->plen > IP6_MAX_PREFIX_LENGTH)
-e75d8
+      return PARSE_ERROR;
     /* Cannot omit data if there is no saved prefix */
     if (tlv->omitted && !state->def_ip6_prefix_seen)
       return PARSE_ERROR;
     /* Merge saved prefix and received prefix parts */
     memcpy(buf, state->def_ip6_prefix, tlv->omitted);
     memcpy(buf + tlv->omitted, tlv->addr, len);
-            4324025f
+    ip6_addr prefix6 = get_ip6(buf);
     net_fill_ip6(&msg->net, prefix6, tlv->plen);
-e75d8
+            Ondrej Zajicek (work)
     if (tlv->flags & BABEL_FLAG_DEF_PREFIX)
+    {
-            4324025f
+      put_ip6(state->def_ip6_prefix, prefix6);
-e75d8
+      state->def_ip6_prefix_seen = 1;
+    }
     if (tlv->flags & BABEL_FLAG_ROUTER_ID)
+    {
-            4324025f
+      state->router_id = ((u64) _I2(prefix6)) << 32 | _I3(prefix6);
-e75d8
+      state->router_id_seen = 1;
+    }
-            4324025f
+            Ondrej Zajicek (work)
     msg->next_hop = state->next_hop_ip6;
-e75d8
+    break;
   case BABEL_AE_IP6_LL:
     /* ??? */
     return PARSE_IGNORE;
   default:
     return PARSE_IGNORE;
+  }
-            ecae2f43
+  /* Update must have Router ID, unless it is retraction */
   if (!state->router_id_seen && (msg->metric != BABEL_INFINITY))
-e75d8
+            Ondrej Zajicek (work)
     DBG("Babel: No router ID seen before update\n");
     return PARSE_ERROR;
+  }
   msg->router_id = state->router_id;
   msg->sender = state->saddr;
-ebfa1
+  state->current_tlv_endpos += len;
-e75d8
+            Ondrej Zajicek (work)
   return PARSE_SUCCESS;
+}
-e236955
+static uint
-e75d8
+babel_write_update(struct babel_tlv *hdr, union babel_msg *m,
-e236955
+                   struct babel_write_state *state, uint max_len)
-e75d8
+            Ondrej Zajicek (work)
   struct babel_msg_update *msg = &m->update;
-e236955
+  uint len0 = 0;
-e75d8
+            Ondrej Zajicek (work)
   /*
    * When needed, we write Router-ID TLV before Update TLV and return size of
    * both of them. There is enough space for the Router-ID TLV, because
    * sizeof(struct babel_tlv_router_id) == sizeof(struct babel_tlv_update).
-d6ca220
+            Ondrej Zajicek (work)
-            4324025f
+   * Router ID is not used for retractions, so do not use it in such case.
-e75d8
+   */
-d6ca220
+  if ((msg->metric < BABEL_INFINITY) &&
       (!state->router_id_seen || (msg->router_id != state->router_id)))
-e75d8
+            Ondrej Zajicek (work)
     len0 = babel_write_router_id(hdr, msg->router_id, state, max_len);
-            4324025f
+    hdr = NEXT_TLV(hdr);
-e75d8
+            Ondrej Zajicek (work)
-            4324025f
+  /*
    * We also may add Next Hop TLV for regular updates. It may fail for not
    * enough space or it may be unnecessary as the next hop is the same as the
    * last one already announced. So we handle all three cases.
    */
   if (msg->metric < BABEL_INFINITY)
+  {
     int l = babel_write_next_hop(hdr, msg->next_hop, state, max_len - len0);
     if (l < 0)
       return 0;
     if (l)
+    {
       len0 += l;
       hdr = NEXT_TLV(hdr);
+    }
+  }
   struct babel_tlv_update *tlv = (void *) hdr;
-e8df049
+  uint len = sizeof(struct babel_tlv_update) + NET_SIZE(&msg->net);
-e75d8
+            Ondrej Zajicek (work)
   if (len0 + len > max_len)
     return 0;
   memset(tlv, 0, sizeof(struct babel_tlv_update));
   TLV_HDR(tlv, BABEL_TLV_UPDATE, len);
-d6ca220
+            Ondrej Zajicek (work)
   if (msg->wildcard)
+  {
     tlv->ae = BABEL_AE_WILDCARD;
     tlv->plen = 0;
+  }
-            4324025f
+  else if (msg->net.type == NET_IP4)
+  {
     tlv->ae = BABEL_AE_IP4;
     tlv->plen = net4_pxlen(&msg->net);
     put_ip4_px(tlv->addr, &msg->net);
+  }
-d6ca220
+  else
+  {
     tlv->ae = BABEL_AE_IP6;
-e8df049
+    tlv->plen = net6_pxlen(&msg->net);
-            b3c6273e
+            Ondrej Zajicek (work)
     /* Address compression - omit initial matching bytes */
     u8 buf[16], omit;
     put_ip6(buf, net6_prefix(&msg->net));
     omit = bytes_equal(buf, state->def_ip6_prefix,
                        MIN(tlv->plen, state->def_ip6_pxlen) / 8);
     if (omit > 0)
+    {
       memcpy(tlv->addr, buf + omit, NET_SIZE(&msg->net) - omit);
       tlv->omitted = omit;
       tlv->length -= omit;
       len -= omit;
+    }
     else
+    {
       put_ip6_px(tlv->addr, &msg->net);
       tlv->flags |= BABEL_FLAG_DEF_PREFIX;
       put_ip6(state->def_ip6_prefix, net6_prefix(&msg->net));
       state->def_ip6_pxlen = tlv->plen;
+    }
-d6ca220
+            Ondrej Zajicek (work)
-e75d8
+  put_time16(&tlv->interval, msg->interval);
   put_u16(&tlv->seqno, msg->seqno);
   put_u16(&tlv->metric, msg->metric);
   return len0 + len;
+}
 static int
 babel_read_route_request(struct babel_tlv *hdr, union babel_msg *m,
-e236955
+                         struct babel_parse_state *state UNUSED)
-e75d8
+            Ondrej Zajicek (work)
   struct babel_tlv_route_request *tlv = (void *) hdr;
   struct babel_msg_route_request *msg = &m->route_request;
   msg->type = BABEL_TLV_ROUTE_REQUEST;
   switch (tlv->ae)
+  {
   case BABEL_AE_WILDCARD:
     /* Wildcard requests must have plen 0 */
     if (tlv->plen > 0)
       return PARSE_ERROR;
     msg->full = 1;
     return PARSE_SUCCESS;
   case BABEL_AE_IP4:
-            4324025f
+    if (tlv->plen > IP4_MAX_PREFIX_LENGTH)
       return PARSE_ERROR;
     if (TLV_OPT_LENGTH(tlv) < BYTES(tlv->plen))
       return PARSE_ERROR;
     read_ip4_px(&msg->net, tlv->addr, tlv->plen);
-ebfa1
+    state->current_tlv_endpos += BYTES(tlv->plen);
-            4324025f
+    return PARSE_SUCCESS;
-e75d8
+            Ondrej Zajicek (work)
   case BABEL_AE_IP6:
-e8df049
+    if (tlv->plen > IP6_MAX_PREFIX_LENGTH)
-e75d8
+      return PARSE_ERROR;
-e236955
+    if (TLV_OPT_LENGTH(tlv) < BYTES(tlv->plen))
-e75d8
+      return PARSE_ERROR;
-e8df049
+    read_ip6_px(&msg->net, tlv->addr, tlv->plen);
-ebfa1
+    state->current_tlv_endpos += BYTES(tlv->plen);
-e75d8
+    return PARSE_SUCCESS;
   case BABEL_AE_IP6_LL:
     return PARSE_ERROR;
   default:
     return PARSE_IGNORE;
+  }
   return PARSE_IGNORE;
+}
-e236955
+static uint
-e75d8
+babel_write_route_request(struct babel_tlv *hdr, union babel_msg *m,
-e236955
+                          struct babel_write_state *state UNUSED, uint max_len)
-e75d8
+            Ondrej Zajicek (work)
   struct babel_tlv_route_request *tlv = (void *) hdr;
   struct babel_msg_route_request *msg = &m->route_request;
-e8df049
+  uint len = sizeof(struct babel_tlv_route_request) + NET_SIZE(&msg->net);
-e75d8
+            Ondrej Zajicek (work)
   if (len > max_len)
     return 0;
   TLV_HDR(tlv, BABEL_TLV_ROUTE_REQUEST, len);
   if (msg->full)
+  {
     tlv->ae = BABEL_AE_WILDCARD;
     tlv->plen = 0;
+  }
-            4324025f
+  else if (msg->net.type == NET_IP4)
+  {
     tlv->ae = BABEL_AE_IP4;
     tlv->plen = net4_pxlen(&msg->net);
     put_ip4_px(tlv->addr, &msg->net);
+  }
-e75d8
+  else
+  {
     tlv->ae = BABEL_AE_IP6;
-e8df049
+    tlv->plen = net6_pxlen(&msg->net);
     put_ip6_px(tlv->addr, &msg->net);
-e75d8
+            Ondrej Zajicek (work)
   return len;
+}
 static int
 babel_read_seqno_request(struct babel_tlv *hdr, union babel_msg *m,
                          struct babel_parse_state *state)
+{
   struct babel_tlv_seqno_request *tlv = (void *) hdr;
   struct babel_msg_seqno_request *msg = &m->seqno_request;
   msg->type = BABEL_TLV_SEQNO_REQUEST;
   msg->seqno = get_u16(&tlv->seqno);
   msg->hop_count = tlv->hop_count;
   msg->router_id = get_u64(&tlv->router_id);
   msg->sender = state->saddr;
   if (tlv->hop_count == 0)
     return PARSE_ERROR;
   switch (tlv->ae)
+  {
   case BABEL_AE_WILDCARD:
     return PARSE_ERROR;
   case BABEL_AE_IP4:
-            4324025f
+    if (tlv->plen > IP4_MAX_PREFIX_LENGTH)
       return PARSE_ERROR;
     if (TLV_OPT_LENGTH(tlv) < BYTES(tlv->plen))
       return PARSE_ERROR;
     read_ip4_px(&msg->net, tlv->addr, tlv->plen);
-ebfa1
+    state->current_tlv_endpos += BYTES(tlv->plen);
-            4324025f
+    return PARSE_SUCCESS;
-e75d8
+            Ondrej Zajicek (work)
   case BABEL_AE_IP6:
-e8df049
+    if (tlv->plen > IP6_MAX_PREFIX_LENGTH)
-e75d8
+      return PARSE_ERROR;
-e236955
+    if (TLV_OPT_LENGTH(tlv) < BYTES(tlv->plen))
-e75d8
+      return PARSE_ERROR;
-e8df049
+    read_ip6_px(&msg->net, tlv->addr, tlv->plen);
-ebfa1
+    state->current_tlv_endpos += BYTES(tlv->plen);
-e75d8
+    return PARSE_SUCCESS;
   case BABEL_AE_IP6_LL:
     return PARSE_ERROR;
   default:
     return PARSE_IGNORE;
+  }
   return PARSE_IGNORE;
+}
-e236955
+static uint
-e75d8
+babel_write_seqno_request(struct babel_tlv *hdr, union babel_msg *m,
-e236955
+                          struct babel_write_state *state UNUSED, uint max_len)
-e75d8
+            Ondrej Zajicek (work)
   struct babel_tlv_seqno_request *tlv = (void *) hdr;
   struct babel_msg_seqno_request *msg = &m->seqno_request;
-e8df049
+  uint len = sizeof(struct babel_tlv_seqno_request) + NET_SIZE(&msg->net);
-e75d8
+            Ondrej Zajicek (work)
   if (len > max_len)
     return 0;
   TLV_HDR(tlv, BABEL_TLV_SEQNO_REQUEST, len);
-            4324025f
+            Ondrej Zajicek (work)
   if (msg->net.type == NET_IP4)
+  {
     tlv->ae = BABEL_AE_IP4;
     tlv->plen = net4_pxlen(&msg->net);
     put_ip4_px(tlv->addr, &msg->net);
+  }
   else
+  {
     tlv->ae = BABEL_AE_IP6;
     tlv->plen = net6_pxlen(&msg->net);
     put_ip6_px(tlv->addr, &msg->net);
+  }
-e75d8
+  put_u16(&tlv->seqno, msg->seqno);
   tlv->hop_count = msg->hop_count;
   put_u64(&tlv->router_id, msg->router_id);
   return len;
+}
 static inline int
-ebfa1
+babel_read_subtlvs(struct babel_tlv *hdr,
                    union babel_msg *msg UNUSED,
                    struct babel_parse_state *state)
+{
   struct babel_tlv *tlv;
   for (tlv = (void *) hdr + state->current_tlv_endpos;
-cb63
+       (void *) tlv < (void *) hdr + TLV_LENGTH(hdr);
-ebfa1
+       tlv = NEXT_TLV(tlv))
+  {
     /*
      * The subtlv type space is non-contiguous (due to the mandatory bit), so
      * use a switch for dispatch instead of the mapping array we use for TLVs
      */
     switch (tlv->type)
+    {
     case BABEL_SUBTLV_PAD1:
     case BABEL_SUBTLV_PADN:
       /* FIXME: Framing errors in PADN are silently ignored, see babel_process_packet() */
       break;
     default:
       /* Unknown mandatory subtlv; PARSE_IGNORE ignores the whole TLV */
       if (tlv->type > 128)
+      {
         DBG("Babel: Mandatory subtlv %d found; skipping TLV\n", tlv->type);
         return PARSE_IGNORE;
+      }
       break;
+    }
+  }
   return PARSE_SUCCESS;
+}
 static inline int
-e75d8
+babel_read_tlv(struct babel_tlv *hdr,
                union babel_msg *msg,
                struct babel_parse_state *state)
+{
   if ((hdr->type <= BABEL_TLV_PADN) ||
       (hdr->type >= BABEL_TLV_MAX) ||
       !tlv_data[hdr->type].read_tlv)
     return PARSE_IGNORE;
   if (TLV_LENGTH(hdr) < tlv_data[hdr->type].min_length)
     return PARSE_ERROR;
-ebfa1
+  state->current_tlv_endpos = tlv_data[hdr->type].min_length;
-e75d8
+  memset(msg, 0, sizeof(*msg));
-ebfa1
+            Ondrej Zajicek (work)
   int res = tlv_data[hdr->type].read_tlv(hdr, msg, state);
   if (res != PARSE_SUCCESS)
     return res;
   return babel_read_subtlvs(hdr, msg, state);
-e75d8
+            Ondrej Zajicek (work)
-e236955
+static uint
-e75d8
+babel_write_tlv(struct babel_tlv *hdr,
                 union babel_msg *msg,
                 struct babel_write_state *state,
-e236955
+                uint max_len)
-e75d8
+            Ondrej Zajicek (work)
   if ((msg->type <= BABEL_TLV_PADN) ||
       (msg->type >= BABEL_TLV_MAX) ||
       !tlv_data[msg->type].write_tlv)
     return 0;
   if (tlv_data[msg->type].min_length > max_len)
     return 0;
   memset(hdr, 0, tlv_data[msg->type].min_length);
   return tlv_data[msg->type].write_tlv(hdr, msg, state, max_len);
+}
 /*
  *        Packet RX/TX functions
  */
 static int
 babel_send_to(struct babel_iface *ifa, ip_addr dest)
+{
   sock *sk = ifa->sk;
   struct babel_pkt_header *hdr = (void *) sk->tbuf;
   int len = get_u16(&hdr->length) + sizeof(struct babel_pkt_header);
   DBG("Babel: Sending %d bytes to %I\n", len, dest);
   return sk_send_to(sk, len, dest, 0);
+}
 /**
  * babel_write_queue - Write a TLV queue to a transmission buffer
  * @ifa: Interface holding the transmission buffer
  * @queue: TLV queue to write (containing internal-format TLVs)
+ *
  * This function writes a packet to the interface transmission buffer with as
  * many TLVs from the &queue as will fit in the buffer. It returns the number of
  * bytes written (NOT counting the packet header). The function is called by
  * babel_send_queue() and babel_send_unicast() to construct packets for
  * transmission, and uses per-TLV helper functions to convert the
  * internal-format TLVs to their wire representations.
+ *
  * The TLVs in the queue are freed after they are written to the buffer.
  */
-e236955
+static uint
-e75d8
+babel_write_queue(struct babel_iface *ifa, list *queue)
+{
   struct babel_proto *p = ifa->proto;
-            4324025f
+  struct babel_write_state state = { .next_hop_ip6 = ifa->addr };
-e75d8
+            Ondrej Zajicek (work)
   if (EMPTY_LIST(*queue))
     return 0;
   byte *pos = ifa->sk->tbuf;
   byte *end = pos + ifa->tx_length;
   struct babel_pkt_header *pkt = (void *) pos;
   pkt->magic = BABEL_MAGIC;
   pkt->version = BABEL_VERSION;
   pkt->length = 0;
   pos += sizeof(struct babel_pkt_header);
   struct babel_msg_node *msg;
   WALK_LIST_FIRST(msg, *queue)
+  {
-e236955
+    if (pos >= end)
       break;
-e75d8
+    int len = babel_write_tlv((struct babel_tlv *) pos, &msg->msg, &state, end - pos);
     if (!len)
       break;
     pos += len;
     rem_node(NODE msg);
     sl_free(p->msg_slab, msg);
+  }
-e236955
+  uint plen = pos - (byte *) pkt;
-e75d8
+  put_u16(&pkt->length, plen - sizeof(struct babel_pkt_header));
   return plen;
+}
 void
 babel_send_queue(void *arg)
+{
   struct babel_iface *ifa = arg;
   while ((babel_write_queue(ifa, &ifa->msg_queue) > 0) &&
          (babel_send_to(ifa, IP6_BABEL_ROUTERS) > 0));
+}
 static inline void
 babel_kick_queue(struct babel_iface *ifa)
+{
   /*
    * Only schedule send event if there is not already data in the socket buffer.
    * Otherwise we may overwrite the data already in the buffer.
    */
   if ((ifa->sk->tpos == ifa->sk->tbuf) && !ev_active(ifa->send_event))
     ev_schedule(ifa->send_event);
+}
 /**
  * babel_send_unicast - send a single TLV via unicast to a destination
  * @msg: TLV to send
  * @ifa: Interface to send via
  * @dest: Destination of the TLV
+ *
  * This function is used to send a single TLV via unicast to a designated
  * receiver. This is used for replying to certain incoming requests, and for
  * sending unicast requests to refresh routes before they expire.
  */
 void
 babel_send_unicast(union babel_msg *msg, struct babel_iface *ifa, ip_addr dest)
+{
   struct babel_proto *p = ifa->proto;
   struct babel_msg_node *msgn = sl_alloc(p->msg_slab);
   list queue;
   msgn->msg = *msg;
   init_list(&queue);
   add_tail(&queue, NODE msgn);
   babel_write_queue(ifa, &queue);
   babel_send_to(ifa, dest);
   /* We could overwrite waiting packet here, we may have to kick TX queue */
   if (!EMPTY_LIST(ifa->msg_queue))
     babel_kick_queue(ifa);
+}
 /**
  * babel_enqueue - enqueue a TLV for transmission on an interface
  * @msg: TLV to enqueue (in internal TLV format)
  * @ifa: Interface to enqueue to
+ *
  * This function is called to enqueue a TLV for subsequent transmission on an
  * interface. The transmission event is triggered whenever a TLV is enqueued;
  * this ensures that TLVs will be transmitted in a timely manner, but that TLVs
  * which are enqueued in rapid succession can be transmitted together in one
  * packet.
  */
 void
 babel_enqueue(union babel_msg *msg, struct babel_iface *ifa)
+{
   struct babel_proto *p = ifa->proto;
   struct babel_msg_node *msgn = sl_alloc(p->msg_slab);
   msgn->msg = *msg;
   add_tail(&ifa->msg_queue, NODE msgn);
   babel_kick_queue(ifa);
+}
 /**
  * babel_process_packet - process incoming data packet
  * @pkt: Pointer to the packet data
  * @len: Length of received packet
  * @saddr: Address of packet sender
  * @ifa: Interface packet was received on.
+ *
  * This function is the main processing hook of incoming Babel packets. It
  * checks that the packet header is well-formed, then processes the TLVs
  * contained in the packet. This is done in two passes: First all TLVs are
  * parsed into the internal TLV format. If a TLV parser fails, processing of the
  * rest of the packet is aborted.
+ *
  * After the parsing step, the TLV handlers are called for each parsed TLV in
  * order.
  */
 static void
 babel_process_packet(struct babel_pkt_header *pkt, int len,
                      ip_addr saddr, struct babel_iface *ifa)
+{
   struct babel_proto *p = ifa->proto;
   struct babel_tlv *tlv;
   struct babel_msg_node *msg;
   list msgs;
   int res;
   int plen = sizeof(struct babel_pkt_header) + get_u16(&pkt->length);
   byte *pos;
   byte *end = (byte *)pkt + plen;
   struct babel_parse_state state = {
-            4324025f
+    .proto          = p,
     .ifa          = ifa,
     .saddr          = saddr,
     .next_hop_ip6 = saddr,
-e75d8
+  };
   if ((pkt->magic != BABEL_MAGIC) || (pkt->version != BABEL_VERSION))
+  {
     TRACE(D_PACKETS, "Strange packet from %I via %s - magic %d version %d",
           saddr, ifa->iface->name, pkt->magic, pkt->version);
     return;
+  }
   if (plen > len)
+  {
     LOG_PKT("Bad packet from %I via %s - %s (%u)",
             saddr, ifa->iface->name, "length mismatch", plen);
     return;
+  }
   TRACE(D_PACKETS, "Packet received from %I via %s",
         saddr, ifa->iface->name);
   init_list(&msgs);
   /* First pass through the packet TLV by TLV, parsing each into internal data
      structures. */
   for (tlv = FIRST_TLV(pkt);
        (byte *)tlv < end;
        tlv = NEXT_TLV(tlv))
+  {
     /* Ugly special case */
     if (tlv->type == BABEL_TLV_PAD1)
       continue;
     /* The end of the common TLV header */
     pos = (byte *)tlv + sizeof(struct babel_tlv);
     if ((pos > end) || (pos + tlv->length > end))
+    {
       LOG_PKT("Bad TLV from %I via %s type %d pos %d - framing error",
               saddr, ifa->iface->name, tlv->type, (byte *)tlv - (byte *)pkt);
       break;
+    }
     msg = sl_alloc(p->msg_slab);
     res = babel_read_tlv(tlv, &msg->msg, &state);
     if (res == PARSE_SUCCESS)
+    {
       add_tail(&msgs, NODE msg);
+    }
     else if (res == PARSE_IGNORE)
+    {
       DBG("Babel: Ignoring TLV of type %d\n", tlv->type);
       sl_free(p->msg_slab, msg);
+    }
     else /* PARSE_ERROR */
+    {
       LOG_PKT("Bad TLV from %I via %s type %d pos %d - parse error",
               saddr, ifa->iface->name, tlv->type, (byte *)tlv - (byte *)pkt);
       sl_free(p->msg_slab, msg);
       break;
+    }
+  }
   /* Parsing done, handle all parsed TLVs */
   WALK_LIST_FIRST(msg, msgs)
+  {
     if (tlv_data[msg->msg.type].handle_tlv)
       tlv_data[msg->msg.type].handle_tlv(&msg->msg, ifa);
     rem_node(NODE msg);
     sl_free(p->msg_slab, msg);
+  }
+}
 static void
 babel_err_hook(sock *sk, int err)
+{
   struct babel_iface *ifa = sk->data;
   struct babel_proto *p = ifa->proto;
   log(L_ERR "%s: Socket error on %s: %M", p->p.name, ifa->iface->name, err);
   /* FIXME: Drop queued TLVs here? */
+}
 static void
 babel_tx_hook(sock *sk)
+{
   struct babel_iface *ifa = sk->data;
   DBG("Babel: TX hook called (iface %s, src %I, dst %I)\n",
       sk->iface->name, sk->saddr, sk->daddr);
   babel_send_queue(ifa);
+}
 static int
-e236955
+babel_rx_hook(sock *sk, uint len)
-e75d8
+            Ondrej Zajicek (work)
   struct babel_iface *ifa = sk->data;
   struct babel_proto *p = ifa->proto;
   const char *err_dsc = NULL;
   uint err_val = 0;
   if (sk->lifindex != ifa->iface->index)
     return 1;
   DBG("Babel: RX hook called (iface %s, src %I, dst %I)\n",
       sk->iface->name, sk->faddr, sk->laddr);
   /* Silently ignore my own packets */
-f02da
+  if (ipa_equal(sk->faddr, sk->saddr))
-e75d8
+    return 1;
   if (!ipa_is_link_local(sk->faddr))
     DROP1("wrong src address");
   if (sk->fport != ifa->cf->port)
     DROP("wrong src port", sk->fport);
   if (len < sizeof(struct babel_pkt_header))
     DROP("too short", len);
   if (sk->flags & SKF_TRUNCATED)
     DROP("truncated", len);
   babel_process_packet((struct babel_pkt_header *) sk->rbuf, len, sk->faddr, ifa);
   return 1;
 drop:
   LOG_PKT("Bad packet from %I via %s - %s (%u)",
           sk->faddr, sk->iface->name, err_dsc, err_val);
   return 1;
+}
 int
 babel_open_socket(struct babel_iface *ifa)
+{
   struct babel_proto *p = ifa->proto;
   sock *sk;
   sk = sk_new(ifa->pool);
   sk->type = SK_UDP;
   sk->sport = ifa->cf->port;
   sk->dport = ifa->cf->port;
   sk->iface = ifa->iface;
-f02da
+  sk->saddr = ifa->addr;
-e75d8
+            Ondrej Zajicek (work)
   sk->rx_hook = babel_rx_hook;
   sk->tx_hook = babel_tx_hook;
   sk->err_hook = babel_err_hook;
   sk->data = ifa;
   sk->tos = ifa->cf->tx_tos;
   sk->priority = ifa->cf->tx_priority;
   sk->ttl = 1;
   sk->flags = SKF_LADDR_RX;
   if (sk_open(sk) < 0)
     goto err;
   if (sk_setup_multicast(sk) < 0)
     goto err;
   if (sk_join_group(sk, IP6_BABEL_ROUTERS) < 0)
     goto err;
   ifa->sk = sk;
   return 1;
 err:
   sk_log_error(sk, p->p.name);
   rfree(sk);
   return 0;
+}