From 659ef444ad3ea6cd470101f99f1267eb7f6fdf4b Mon Sep 17 00:00:00 2001
From: Mikael Magnusson <mikma@users.sourceforge.net>
Date: Sat, 9 Mar 2019 00:05:15 +0100
Subject: Wireguard: Extended color community

---
 proto/wireguard/wireguard.c | 59 ++++++++++++++++++++++++++++++++++++---------
 1 file changed, 47 insertions(+), 12 deletions(-)

diff --git a/proto/wireguard/wireguard.c b/proto/wireguard/wireguard.c
index 2882c862..c7345c46 100644
--- a/proto/wireguard/wireguard.c
+++ b/proto/wireguard/wireguard.c
@@ -50,10 +50,15 @@ dump(void *ptr, size_t len)
 #define BGP_TUNNEL_ENCAP_A_SUB_TLV_REMOTE_EP     6
 #define BGP_TUNNEL_ENCAP_A_SUB_TLV_UDP_DEST_PORT 8
 
+#define FLAG_BGP_TUNNEL_ENCAP_A_SUB_TLV_ENCAP         (1<<BGP_TUNNEL_ENCAP_A_SUB_TLV_ENCAP)
+#define FLAG_BGP_TUNNEL_ENCAP_A_SUB_TLV_COLOR         (1<<BGP_TUNNEL_ENCAP_A_SUB_TLV_COLOR)
+#define FLAG_BGP_TUNNEL_ENCAP_A_SUB_TLV_REMOTE_EP     (1<<BGP_TUNNEL_ENCAP_A_SUB_TLV_REMOTE_EP)
+#define FLAG_BGP_TUNNEL_ENCAP_A_SUB_TLV_UDP_DEST_PORT (1<<BGP_TUNNEL_ENCAP_A_SUB_TLV_UDP_DEST_PORT)
+
 #define BGP_TUNNEL_ENCAP_A_TUNNEL_TYPE_WIREGUARD 65535
 
 static
-int decode_wireguard(const void *p, size_t sub_tlv_len, wg_key *pubkey)
+int decode_wireguard(const void *p, size_t sub_tlv_len, wg_key *pubkey, u16 *flags)
 {
   if (sub_tlv_len != sizeof(wg_key)) {
     log(L_TRACE "WG: wireguard len error %d", sub_tlv_len);
@@ -61,11 +66,30 @@ int decode_wireguard(const void *p, size_t sub_tlv_len, wg_key *pubkey)
   }
 
   memcpy(pubkey, p, sizeof(wg_key));
+  *flags |= FLAG_BGP_TUNNEL_ENCAP_A_SUB_TLV_ENCAP;
+  return 0;
+}
+
+static
+int decode_color(const void *p, size_t sub_tlv_len, u32 *color, u16 *flags)
+{
+  if (sub_tlv_len != 8) {
+    log(L_TRACE "WG: color len error %d", sub_tlv_len);
+    return -1;
+  }
+
+  if (get_u16(p) != 0x030b) {
+    log(L_TRACE "WG: color error %04x", get_u16(p));
+    return -1;
+  }
+
+  *color = get_u32(p+2);
+  *flags |= FLAG_BGP_TUNNEL_ENCAP_A_SUB_TLV_COLOR;
   return 0;
 }
 
 static
-int decode_udp_dest_port(const void *p, size_t sub_tlv_len, u16 *udp_dest_port)
+int decode_udp_dest_port(const void *p, size_t sub_tlv_len, u16 *udp_dest_port, u16 *flags)
 {
   if (sub_tlv_len != 2) {
     log(L_TRACE "WG: udp dest port len error %d", sub_tlv_len);
@@ -73,11 +97,12 @@ int decode_udp_dest_port(const void *p, size_t sub_tlv_len, u16 *udp_dest_port)
   }
 
   *udp_dest_port = get_u16(p);
+  *flags |= FLAG_BGP_TUNNEL_ENCAP_A_SUB_TLV_UDP_DEST_PORT;
   return 0;
 }
 
 static
-int decode_remote_ep(void *p, size_t sub_tlv_len, u32 *as4, ip_addr *remote_ep)
+int decode_remote_ep(void *p, size_t sub_tlv_len, u32 *as4, ip_addr *remote_ep, u16 *flags)
 {
   if (sub_tlv_len < 6) {
     log(L_TRACE "WG: remote ep len error");
@@ -93,6 +118,7 @@ int decode_remote_ep(void *p, size_t sub_tlv_len, u32 *as4, ip_addr *remote_ep)
       return -1;
     }
     *remote_ep = ipa_from_ip4(get_ip4(p + 6));
+    *flags |= FLAG_BGP_TUNNEL_ENCAP_A_SUB_TLV_REMOTE_EP;
     return 0;
   case NET_IP6:
     if (sub_tlv_len != 22) {
@@ -100,6 +126,7 @@ int decode_remote_ep(void *p, size_t sub_tlv_len, u32 *as4, ip_addr *remote_ep)
       return -1;
     }
     *remote_ep = ipa_from_ip6(get_ip6(p + 6));
+    *flags |= FLAG_BGP_TUNNEL_ENCAP_A_SUB_TLV_REMOTE_EP;
     return 0;
   default:
     log(L_TRACE "WG: Address family error %d", af);
@@ -109,7 +136,8 @@ int decode_remote_ep(void *p, size_t sub_tlv_len, u32 *as4, ip_addr *remote_ep)
 
 static
 int decode_sub_tlv(u8 *p, size_t len, wg_key *pubkey,
-		   u32 *remote_ep_as, ip_addr *remote_ep_addr, u16 *udp_dest_port)
+		   u32 *remote_ep_as, ip_addr *remote_ep_addr,
+		   u32 *color, u16 *udp_dest_port, u16 *flags)
 {
   if (len < 3) {
     log(L_TRACE "WG: sub_tlv len error %d", len);
@@ -143,13 +171,16 @@ int decode_sub_tlv(u8 *p, size_t len, wg_key *pubkey,
 
   switch (type) {
   case BGP_TUNNEL_ENCAP_A_SUB_TLV_ENCAP:
-    res = decode_wireguard(p, sub_tlv_len, pubkey);
+    res = decode_wireguard(p, sub_tlv_len, pubkey, flags);
     break;
   case BGP_TUNNEL_ENCAP_A_SUB_TLV_REMOTE_EP:
-    res = decode_remote_ep(p, sub_tlv_len, remote_ep_as, remote_ep_addr);
+    res = decode_remote_ep(p, sub_tlv_len, remote_ep_as, remote_ep_addr, flags);
+    break;
+  case BGP_TUNNEL_ENCAP_A_SUB_TLV_COLOR:
+    res = decode_color(p, sub_tlv_len, color, flags);
     break;
   case BGP_TUNNEL_ENCAP_A_SUB_TLV_UDP_DEST_PORT:
-    res = decode_udp_dest_port(p, sub_tlv_len, udp_dest_port);
+    res = decode_udp_dest_port(p, sub_tlv_len, udp_dest_port, flags);
     break;
   default:
     /* Skip unsupported sub-TLV. */
@@ -164,7 +195,7 @@ int decode_sub_tlv(u8 *p, size_t len, wg_key *pubkey,
 }
 
 static
-int decode_tunnel_encap(const eattr *e, wg_key *pubkey, u32 *as4, ip_addr *remote_ep, u16 *udp_port)
+int decode_tunnel_encap(const eattr *e, wg_key *pubkey, u32 *as4, ip_addr *remote_ep, u32 *color, u16 *udp_port, u16 *flags)
 {
   u8 *p = e->u.ptr->data;
   int len = e->u.ptr->length;
@@ -193,10 +224,12 @@ int decode_tunnel_encap(const eattr *e, wg_key *pubkey, u32 *as4, ip_addr *remot
   }
 
   for (u8 *cur = p + 4; cur < p + 4 + value_length;) {
-    int res = decode_sub_tlv(cur, value_length, pubkey, as4, remote_ep, udp_port);
+    int res = decode_sub_tlv(cur, value_length, pubkey, as4, remote_ep, color, udp_port, flags);
 
-    if (res < 0)
+    if (res < 0) {
+      log(L_TRACE "WG: decode error %d", res);
       return res;
+    }
 
     cur += res;
   }
@@ -297,10 +330,12 @@ wg_rt_notify(struct proto *P, struct channel *ch UNUSED, struct network *n,
       u32 remote_ep_as4 = 0;
       ip_addr remote_ep_addr = IPA_NONE;
       u16 udp_dest_port = 0;
+      u32 color = 0;
+      u16 flags = 0;
 
       t = ea_find(new->attrs->eattrs, EA_CODE(PROTOCOL_BGP, BA_TUNNEL_ENCAP));
-      if (t && t->u.ptr && decode_tunnel_encap(t, &pubkey, &remote_ep_as4, &remote_ep_addr, &udp_dest_port) == 0) {
-	log(L_TRACE "WG:     Attr %x %x %d", t->flags, t->type, t->u.ptr->length);
+      if (t && t->u.ptr && decode_tunnel_encap(t, &pubkey, &remote_ep_as4, &remote_ep_addr, &color, &udp_dest_port, &flags) == 0) {
+	log(L_TRACE "WG:     Attr %x %x %d %04x", t->flags, t->type, t->u.ptr->length, flags);
 
 	struct wg_device *dev = NULL;
 
-- 
cgit v1.2.3