Wireguard: Improve format function

2 files changed, 78 insertions, 4 deletions

diff --git a/lib/tunnel_encaps.c b/lib/tunnel_encaps.c
index 5b72cb61..421c0c83 100644
--- a/lib/tunnel_encaps.c
+++ b/lib/tunnel_encaps.c
@@ -4,9 +4,14 @@
 static
 int decode_encap(const void *p, size_t sub_tlv_len, struct tunnel_encap *encap, struct pool *pool)
 {
-  mb_free(encap->encap);
-  encap->encap = mb_alloc(pool, sub_tlv_len);
-  memcpy(encap->encap, p, sub_tlv_len);
+  if (pool && !encap->encap) {
+    encap->encap = mb_alloc(pool, sub_tlv_len);
+    encap->encap_len = sub_tlv_len;
+  }
+  if (encap->encap_len < sub_tlv_len) {
+    return -1;
+  }
+  memcpy(encap->encap, p, encap->encap_len);
   encap->encap_len = sub_tlv_len;
   encap->flags |= FLAG_BGP_TUNNEL_ENCAP_A_SUB_TLV_ENCAP;
   return 0;
@@ -25,7 +30,7 @@ int decode_color(const void *p, size_t sub_tlv_len, struct tunnel_encap *encap)
     return -1;
   }
 
-  encap->color = get_u32(p+2);
+  encap->color = get_u32(p+4);
   encap->flags |= FLAG_BGP_TUNNEL_ENCAP_A_SUB_TLV_COLOR;
   return 0;
 }
@@ -170,3 +175,70 @@ int decode_tunnel_encap(const eattr *e, struct tunnel_encap *encap, struct pool
 
   return 0;
 }
+
+int format_tunnel_encap(const eattr *a, byte *buf, uint size)
+{
+  byte *pos = buf;
+  int l;
+  struct tunnel_encap encap;
+  memset(&encap, 0, sizeof(encap));
+  encap.ep.ip = IPA_NONE;
+  encap.encap_len = a->u.ptr->length;
+  encap.encap = alloca(encap.encap_len); /* Guaranteed to be enough */
+
+  if (decode_tunnel_encap(a, &encap, NULL) < 0) {
+    l = bsnprintf(pos, size, "(invalid)");
+    ADVANCE(pos, size, l);
+    return pos - buf;
+  }
+
+  l = bsnprintf(pos, size, "type: %d", encap.type);
+  if (l < 0)
+    return pos - buf;
+  ADVANCE(pos, size, l);
+
+  if (encap.flags & FLAG_BGP_TUNNEL_ENCAP_A_SUB_TLV_TUNNEL_EP) {
+    char tmp[20] = "";
+    if (encap.ep.asn) {
+      bsnprintf(tmp, sizeof(tmp), "[AS%d]:", encap.ep.asn);
+    }
+    l = bsnprintf(pos, size, " ep: %s%I", tmp, encap.ep.ip);
+    if (l < 0)
+      return pos - buf;
+    ADVANCE(pos, size, l);
+  }
+
+  if (encap.flags & FLAG_BGP_TUNNEL_ENCAP_A_SUB_TLV_UDP_DEST_PORT) {
+    l = bsnprintf(pos, size, " udp: %d", encap.udp_dest_port);
+    if (l < 0)
+      return pos - buf;
+    ADVANCE(pos, size, l);
+  }
+
+  if (encap.flags & FLAG_BGP_TUNNEL_ENCAP_A_SUB_TLV_COLOR) {
+    l = bsnprintf(pos, size, " color: %d", encap.color);
+    if (l < 0)
+      return pos - buf;
+    ADVANCE(pos, size, l);
+  }
+
+  if (encap.flags & FLAG_BGP_TUNNEL_ENCAP_A_SUB_TLV_ENCAP) {
+    byte *data = encap.encap;
+
+    l = bsnprintf(pos, size, " encap: ");
+    if (l < 0)
+      return pos - buf;
+    ADVANCE(pos, size, l);
+
+    for (uint i = 0; i < encap.encap_len; i++) {
+      if (size < 4) {
+	return pos - buf;
+      }
+
+      l = bsprintf(pos, "%02x ", data[i]);
+      ADVANCE(pos, size, l);
+    }
+  }
+
+  return pos - buf;
+}
diff --git a/lib/tunnel_encaps.h b/lib/tunnel_encaps.h
index 341dcf22..7358947e 100644
--- a/lib/tunnel_encaps.h
+++ b/lib/tunnel_encaps.h
@@ -34,4 +34,6 @@ struct tunnel_encap {
 
 int decode_tunnel_encap(const eattr *e, struct tunnel_encap *encap, struct pool *pool);
 
+int format_tunnel_encap(const eattr *a, byte *buf, uint size);
+
 #endif	/* _BIRD_TUNNEL_ENCAPS_ */