Wireguard: Improve format function

3 files changed, 81 insertions, 20 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_ */
diff --git a/proto/bgp/attrs.c b/proto/bgp/attrs.c
index dec58cbd..2f4472b1 100644
--- a/proto/bgp/attrs.c
+++ b/proto/bgp/attrs.c
@@ -21,6 +21,7 @@
 #include "lib/resource.h"
 #include "lib/string.h"
 #include "lib/unaligned.h"
+#include "lib/tunnel_encaps.h"
 
 #include "bgp.h"
 
@@ -891,23 +892,9 @@ bgp_format_tunnel_encap(eattr *a, byte *buf, uint size)
 {
   char *pos = buf;
 
-  if (a->u.ptr->length == 0)
-    {
-      bsprintf(pos, "");
-      return;
-    }
-
-  for (uint i = 0; i < a->u.ptr->length; i++)
-  {
-    if (size < 4)
-    {
-      bsprintf(pos, "...");
-      return;
-    }
-
-    uint l = bsprintf(pos, "%02x ", a->u.ptr->data[i]);
+  int l = format_tunnel_encap(a, pos, size);
+  if (l > 0)
     ADVANCE(pos, size, l);
-  }
 }
 
 static void