Wireguard: Refactor tunnel encaps decoding

1 files changed, 1 insertions, 190 deletions

diff --git a/proto/wireguard/wireguard.c b/proto/wireguard/wireguard.c
index fa6ccb0e..212fc779 100644
--- a/proto/wireguard/wireguard.c
+++ b/proto/wireguard/wireguard.c
@@ -8,6 +8,7 @@
 #include <unistd.h>
 #include "lib/lists.h"
 #include "lib/ip.h"
+#include "lib/tunnel_encaps.h"
 #include "nest/protocol.h"
 #include "nest/iface.h"
 #include "sysdep/linux/wireguard.h"
@@ -156,196 +157,6 @@ 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_TUNNEL_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_TUNNEL_EP     (1<<BGP_TUNNEL_ENCAP_A_SUB_TLV_TUNNEL_EP)
-#define FLAG_BGP_TUNNEL_ENCAP_A_SUB_TLV_UDP_DEST_PORT (1<<BGP_TUNNEL_ENCAP_A_SUB_TLV_UDP_DEST_PORT)
-
-static
-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);
-    return -1;
-  }
-
-  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, u16 *flags)
-{
-  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);
-  *flags |= FLAG_BGP_TUNNEL_ENCAP_A_SUB_TLV_UDP_DEST_PORT;
-  return 0;
-}
-
-static
-int decode_tunnel_ep(const void *p, size_t sub_tlv_len, u32 *as4, ip_addr *tunnel_ep, u16 *flags)
-{
-  if (sub_tlv_len < 6) {
-    log(L_TRACE "WG: tunnel 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;
-    }
-    *tunnel_ep = ipa_from_ip4(get_ip4(p + 6));
-    *flags |= FLAG_BGP_TUNNEL_ENCAP_A_SUB_TLV_TUNNEL_EP;
-    return 0;
-  case NET_IP6:
-    if (sub_tlv_len != 22) {
-      log(L_TRACE "WG: IPv6 len error %d", sub_tlv_len);
-      return -1;
-    }
-    *tunnel_ep = ipa_from_ip6(get_ip6(p + 6));
-    *flags |= FLAG_BGP_TUNNEL_ENCAP_A_SUB_TLV_TUNNEL_EP;
-    return 0;
-  default:
-    log(L_TRACE "WG: Address family error %d", af);
-    return -1;
-  }
-}
-
-static
-int decode_sub_tlv(const u8 *p, size_t len, wg_key *pubkey,
-		   u32 *tunnel_ep_as, ip_addr *tunnel_ep_addr,
-		   u32 *color, u16 *udp_dest_port, u16 *flags)
-{
-  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, flags);
-    break;
-  case BGP_TUNNEL_ENCAP_A_SUB_TLV_TUNNEL_EP:
-    res = decode_tunnel_ep(p, sub_tlv_len, tunnel_ep_as, tunnel_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, flags);
-    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, u16 wg_tunnel_type, wg_key *pubkey, u32 *as4, ip_addr *tunnel_ep, u32 *color, u16 *udp_port, u16 *flags)
-{
-  const 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 != wg_tunnel_type) {
-    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 (const u8 *cur = p + 4; cur < p + 4 + value_length;) {
-    int res = decode_sub_tlv(cur, value_length, pubkey, as4, tunnel_ep, color, udp_port, flags);
-
-    if (res < 0) {
-      log(L_TRACE "WG: decode error %d", res);
-      return res;
-    }
-
-    cur += res;
-  }
-
-  return 0;
-}
-
 static wg_peer *
 add_peer(wg_device *dev, wg_key pubkey)
 {