Wireguard: Implement tunnel encode decode

1 files changed, 167 insertions, 2 deletions

diff --git a/proto/wireguard/wireguard.c b/proto/wireguard/wireguard.c
index 0d6bd6a9..8e3c0df4 100644
--- a/proto/wireguard/wireguard.c
+++ b/proto/wireguard/wireguard.c
@@ -45,6 +45,165 @@ dump(void *ptr, size_t len)
   fprintf(stderr, "\n");
 }
 
+#define BGP_TUNNEL_ENCAP_A_SUB_TLV_ENCAP         1
+#define BGP_TUNNEL_ENCAP_A_SUB_TLV_COLOR         4
+#define BGP_TUNNEL_ENCAP_A_SUB_TLV_REMOTE_EP     6
+#define BGP_TUNNEL_ENCAP_A_SUB_TLV_UDP_DEST_PORT 8
+
+#define BGP_TUNNEL_ENCAP_A_TUNNEL_TYPE_WIREGUARD 65535
+
+static
+int decode_wireguard(const void *p, size_t sub_tlv_len, wg_key *pubkey)
+{
+  if (sub_tlv_len != sizeof(wg_key)) {
+    log(L_TRACE "WG: wireguard len error %d", sub_tlv_len);
+    return -1;
+  }
+
+  memcpy(pubkey, p, sizeof(wg_key));
+  return 0;
+}
+
+static
+int decode_udp_dest_port(const void *p, size_t sub_tlv_len, u16 *udp_dest_port)
+{
+  if (sub_tlv_len != 2) {
+    log(L_TRACE "WG: udp dest port len error %d", sub_tlv_len);
+    return -1;
+  }
+
+  *udp_dest_port = get_u16(p);
+  return 0;
+}
+
+static
+int decode_remote_ep(void *p, size_t sub_tlv_len, u32 *as4, ip_addr *remote_ep)
+{
+  if (sub_tlv_len < 6) {
+    log(L_TRACE "WG: remote ep len error");
+    return -1;
+  }
+
+  *as4 = get_u32(p);
+  u16 af = get_u16(p + 4);
+  switch (af) {
+  case NET_IP4:
+    if (sub_tlv_len != 10) {
+      log(L_TRACE "WG: IPv4 len error %d", sub_tlv_len);
+      return -1;
+    }
+    *remote_ep = ipa_from_ip4(get_ip4(p + 6));
+    return 0;
+  case NET_IP6:
+    if (sub_tlv_len != 22) {
+      log(L_TRACE "WG: IPv6 len error %d", sub_tlv_len);
+      return -1;
+    }
+    *remote_ep = ipa_from_ip6(get_ip6(p + 6));
+    return 0;
+  default:
+    log(L_TRACE "WG: Address family error %d", af);
+    return -1;
+  }
+}
+
+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)
+{
+  if (len < 3) {
+    log(L_TRACE "WG: sub_tlv len error %d", len);
+    return -1;
+  }
+
+  const u8 *first = p;
+  const u8 *last = p + len;
+  int type = get_u8(p++);
+  u16 sub_tlv_len = 0;
+
+  log(L_TRACE "WG: sub tlv type %d", type);
+  if (type >= 0 && type <= 127) {
+    sub_tlv_len = get_u8(p);
+    p++;
+  } else if (type >= 128 && type <= 255) {
+    sub_tlv_len = get_u16(p);
+    p += 2;
+  } else {
+    log(L_TRACE "WG: sub_tlv type error %d", type);
+    return -1;
+  }
+
+  log(L_TRACE "WG: sub tlv len %d", sub_tlv_len);
+  if (p + sub_tlv_len > last) {
+    log(L_TRACE "WG: sub_tlv value len error %d", sub_tlv_len);
+    return -1;
+  }
+
+  int res = 0;
+
+  switch (type) {
+  case BGP_TUNNEL_ENCAP_A_SUB_TLV_ENCAP:
+    res = decode_wireguard(p, sub_tlv_len, pubkey);
+    break;
+  case BGP_TUNNEL_ENCAP_A_SUB_TLV_REMOTE_EP:
+    res = decode_remote_ep(p, sub_tlv_len, remote_ep_as, remote_ep_addr);
+    break;
+  case BGP_TUNNEL_ENCAP_A_SUB_TLV_UDP_DEST_PORT:
+    res = decode_udp_dest_port(p, sub_tlv_len, udp_dest_port);
+    break;
+  default:
+    /* Skip unsupported sub-TLV. */
+    res = 0;
+    break;
+  }
+
+  if (res < 0)
+    return res;
+
+  return p - first + sub_tlv_len;
+}
+
+static
+int decode_tunnel_encap(const eattr *e, wg_key *pubkey, u32 *as4, ip_addr *remote_ep, u16 *udp_port)
+{
+  u8 *p = e->u.ptr->data;
+  int len = e->u.ptr->length;
+
+  if (len < 4) {
+    log(L_TRACE "WG: tunnel_encap len error %d", len);
+    return -1;
+  }
+
+  u16 tunnel_type = get_u16(p);
+
+  log(L_DEBUG "WG: tunnel type %d", tunnel_type);
+
+  if (tunnel_type != BGP_TUNNEL_ENCAP_A_TUNNEL_TYPE_WIREGUARD) {
+    log(L_TRACE "WG: tunnel type error %d", tunnel_type);
+    return -1;
+  }
+
+  u16 value_length = get_u16(p + 2);
+
+  log(L_TRACE "WG: tunnel encap value len %d", value_length);
+
+  if (len < value_length + 4) {
+    log(L_TRACE "WG: tunnel encap len error %d", value_length);
+    return -1;
+  }
+
+  for (u8 *cur = p + 4; cur < p + 4 + value_length;) {
+    int res = decode_sub_tlv(cur, value_length, pubkey, as4, remote_ep, udp_port);
+
+    if (res < 0)
+      return res;
+
+    cur += res;
+  }
+
+  return 0;
+}
+
 static void
 wg_rt_notify(struct proto *P, struct channel *ch UNUSED, struct network *n,
   struct rte *new, struct rte *old UNUSED)
@@ -70,8 +229,14 @@ wg_rt_notify(struct proto *P, struct channel *ch UNUSED, struct network *n,
       debug("WG: notify new %d %N\n",
         new->attrs->dest, n->n.addr);
 
+      wg_key pubkey;
+      memset(pubkey, 0, sizeof(wg_key));
+      u32 remote_ep_as4 = 0;
+      ip_addr remote_ep_addr = IPA_NONE;
+      u16 udp_dest_port = 0;
+
       t = ea_find(new->attrs->eattrs, EA_CODE(PROTOCOL_BGP, BA_TUNNEL_ENCAP));
-      if (t && t->u.ptr) {
+      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);
 
 	struct wg_device *dev = NULL;
@@ -83,7 +248,7 @@ wg_rt_notify(struct proto *P, struct channel *ch UNUSED, struct network *n,
 	    // Look for public key
 	    size_t len = 32; // FIXME
 	    // MIN(32, t->u.ptr->length)
-	    if (memcmp(peer->public_key, &t->u.ptr->data[t->u.ptr->length - 32], len) != 0) {
+	    if (memcmp(peer->public_key, pubkey, len) != 0) {
 	      log(L_TRACE "WG:     Not found");
 	      continue;
 	    }