diff options
| -rw-r--r-- | proto/wireguard/wireguard.c | 169 |
1 files changed, 167 insertions, 2 deletions
diff --git a/proto/wireguard/wireguard.c b/proto/wireguard/wireguard.c index 7f50ca75..a2769138 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 AF_INET: + 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 AF_INET6: + 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; } |