path: root/proto/wireguard/wireguard.c

// Based on proto/rip/rip.c

#include <stdio.h>
#include <stdlib.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/un.h>
#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"
#include "sysdep/unix/unix.h"
#include "sysdep/unix/wg_user.h"
#include "wireguard.h"

static ip_addr allowedip_to_ipa(struct wg_allowedip *allowedip);

static
int get_device(struct wg_proto *p, wg_device **pdev, const char *device_name)
{
  struct wg_config *c = (struct wg_config *) p->p.cf;

  /* if (has_user_space(p)) */
    /* return user_get_device(p, dev, device_name); */
  /* else */
  /*   return wg_get_device(dev, device_name); */

  if (p->dev)
    {
      wg_free_device(p->dev);
      p->dev = NULL;
    }

  wg_device *dev = calloc(1, sizeof(wg_device));
  strncpy(dev->name, device_name, sizeof(dev->name));
  dev->flags = WGDEVICE_REPLACE_PEERS;
  if (c->private_key)
    {
      dev->flags |= WGDEVICE_HAS_PRIVATE_KEY | WGDEVICE_HAS_PUBLIC_KEY;
      wg_key_from_base64(dev->private_key, c->private_key);
      wg_generate_public_key(dev->public_key, dev->private_key);
    }
  if (c->listen_port)
    dev->flags |= WGDEVICE_HAS_LISTEN_PORT;
  dev->listen_port = c->listen_port;
  debug("listen port %d\n", c->listen_port);

  struct peer_config *pc = NULL;
  WALK_LIST(pc,c->peers)
    {
      wg_peer *peer = calloc(1, sizeof(wg_peer));
      if (!dev->first_peer)
	dev->first_peer = peer;
      if (dev->last_peer)
	dev->last_peer->next_peer = peer;
      dev->last_peer = peer;

      peer->flags = WGPEER_REPLACE_ALLOWEDIPS;

      if (pc->public_key)
	{
	  peer->flags = WGPEER_HAS_PUBLIC_KEY;
	  wg_key_from_base64(peer->public_key, pc->public_key);
	}
      peer->next_peer = NULL;

      sockaddr_fill((sockaddr*)&peer->endpoint.addr,
		    ipa_is_ip4(pc->endpoint) ? AF_INET : AF_INET6,
		    pc->endpoint, NULL, pc->remote_port);

      wg_allowedip *allowedip = calloc(1, sizeof(wg_allowedip));
      peer->first_allowedip = allowedip;
      peer->last_allowedip = allowedip;

      switch (pc->allowedip.type)
	{
	case NET_IP4:
	  allowedip->family = AF_INET;
	  allowedip->ip4 = ipa_to_in4(net_prefix(&pc->allowedip));
	  allowedip->cidr = net_pxlen(&pc->allowedip);
	  break;
	case NET_IP6:
	  allowedip->family = AF_INET6;
	  allowedip->ip6 = ipa_to_in6(net_prefix(&pc->allowedip));
	  allowedip->cidr = net_pxlen(&pc->allowedip);
	  break;
	default:
	  break;
	}

      allowedip->next_allowedip = NULL;
    }

  *pdev = dev;
  return 0;
}

static int
set_device(struct wg_proto *p)
{
  struct wg_config *c = (struct wg_config *) p->p.cf;

  if (wg_has_userspace(c->ifname))
    return wg_user_set_device(p->p.pool, c->ifname, p->dev);
  else
    {
      log(L_TRACE "WG: wg_set_device");
      return wg_set_device(p->dev);
    }
}

static void
wg_init_entry(void *e_)
{
  struct wg_entry *e UNUSED = e_;
//  debug("wg_init_entry\n");

  memset(e, 0, sizeof(struct wg_entry) - sizeof(struct fib_node));
}

static void
wg_postconfig(struct proto_config *C UNUSED)
{
  debug("postconfig\n");
}

static void
wg_if_notify(struct proto *P, unsigned flags, struct iface *i)
{
  struct wg_proto *p = (struct wg_proto *) P;
  struct wg_config *c = (struct wg_config *) P->cf;

  debug("WG: if_notify %p %s %d %s\n", i, i->name, flags, c->ifname);
  if (c->ifname && !strcmp(i->name, c->ifname)) {
    debug("WG: found ifname\n");
    p->iface = i;
  }

  if (flags & IF_CHANGE_UP)
    {
      debug("WG: IF_CHANGE_UP %s\n", i->name);

      int res = set_device(p);
      log(L_TRACE "WG: wg_set_device %d", res);
    }
}

static void
dump(void *ptr, size_t len)
{
  unsigned char *data = ptr;

  for (size_t i=0; i<len; i++) {
    fprintf(stderr, "%02x ", data[i]);
  }
  fprintf(stderr, "\n");
}

static void
dump_peer(struct wg_peer *peer)
{
  wg_key_b64_string base64;
  wg_key_to_base64(base64, peer->public_key);
  debug("WG: peer %s\n", base64);

  struct wg_allowedip *allowedip = NULL;
  wg_for_each_allowedip(peer, allowedip) {
    ip_addr ip = allowedip_to_ipa(allowedip);

    debug("allowedip %I/%d\n", ip, allowedip->cidr);
  }
}

static wg_peer *
add_peer(wg_device *dev, const wg_key pubkey)
{
  struct wg_peer *peer = malloc(sizeof(struct wg_peer));
  memset(peer, 0, sizeof(struct wg_peer));

  peer->flags = WGPEER_HAS_PUBLIC_KEY;
  memcpy(peer->public_key, pubkey, sizeof(wg_key));

  if (dev->first_peer && dev->last_peer)
    dev->last_peer->next_peer = peer;
  else
    dev->first_peer = peer;
  dev->last_peer = peer;
  return peer;
}

static void
remove_marked_peer(wg_device *dev)
{
  struct wg_peer *peer = NULL;
  struct wg_peer *prevpeer = NULL;

  log(L_TRACE "WG: remove_marked_peer");
  wg_for_each_peer(dev, peer) {
    if (peer->flags & WGPEER_REMOVE_ME) {
      if (!prevpeer) {
	debug("WG: remove first peer\n");
	dev->first_peer = peer->next_peer;
	if (dev->last_peer == peer) {
	  dev->last_peer = NULL;
	  dev->first_peer = NULL;
	}
      } else {
	debug("WG: remove middle peer\n");
	// Remove
	if (dev->last_peer == peer)
	  dev->last_peer = prevpeer;

	prevpeer->next_peer = peer->next_peer;
      }

      free(peer);
      return;
    }

    prevpeer = peer;
  }
  log(L_WARN "WG: marked peer not found");
}

static int
set_peer_tunnel_ep(wg_peer *peer, ip_addr tunnel_ep_addr, u16 udp_dest_port)
{
  if (udp_dest_port != 0 && ipa_nonzero(tunnel_ep_addr) ) {
    if (ipa_is_ip4(tunnel_ep_addr)) {
      log(L_TRACE "WG:     found ip4 ep");
      peer->endpoint.addr4.sin_family = AF_INET;
      put_ip4(&peer->endpoint.addr4.sin_addr.s_addr, ipa_to_ip4(tunnel_ep_addr));
      put_u16(&peer->endpoint.addr4.sin_port, udp_dest_port);
    } else {
      log(L_TRACE "WG:     found ip6 ep");
      peer->endpoint.addr6.sin6_family = AF_INET6;
      put_ip6(&peer->endpoint.addr6.sin6_addr, ipa_to_ip6(tunnel_ep_addr));
      put_u16(&peer->endpoint.addr6.sin6_port, udp_dest_port);
    }
  }

  return 0;
}

static ip_addr
allowedip_to_ipa(struct wg_allowedip *allowedip)
{
  switch (allowedip->family) {
  case AF_INET:
    return ipa_from_in4(allowedip->ip4);
    break;
  case AF_INET6:
    return ipa_from_in6(allowedip->ip6);
  }

  return IPA_NONE;
}

static void
init_allowed_ip(struct wg_allowedip *allowedip, u8 net_type, struct network *n)
{
  memset(allowedip, 0, sizeof(struct wg_allowedip));

  if (net_type == NET_IP4) {
    allowedip->family = AF_INET;
    allowedip->ip4.s_addr = ip4_to_u32(ip4_hton(net4_prefix(n->n.addr)));
  } else if (net_type == NET_IP6) {
    allowedip->family = AF_INET6;
    ip6_addr addr = ip6_hton(net6_prefix(n->n.addr));
    memcpy(allowedip->ip6.s6_addr, &addr, 16);
  }

  allowedip->cidr = net_pxlen(n->n.addr);
}

static int
add_allowed_ip(u8 net_type, struct network *n, wg_peer *peer)
{
  // Add allowed ip
  struct wg_allowedip *allowedip = malloc(sizeof(struct wg_allowedip));
  init_allowed_ip(allowedip, net_type, n);

  if (peer->first_allowedip && peer->last_allowedip)
    peer->last_allowedip->next_allowedip = allowedip;
  else
    peer->first_allowedip = allowedip;
  peer->last_allowedip = allowedip;

  return 0;
}

static bool
remove_allowed_ip(wg_peer *peer, struct wg_allowedip *allowedip)
{
  struct wg_allowedip *ip = NULL;
  struct wg_allowedip *previp = NULL;

  wg_for_each_allowedip(peer, ip) {
    if (allowedip->family != ip->family) {
      debug("WG: family no match\n");
      previp = ip;
      continue;
    }

    if (allowedip->cidr != ip->cidr) {
      debug("WG: cidr no match\n");
      previp = ip;
      continue;
    }

    if (memcmp(&allowedip->ip6, &ip->ip6, sizeof(struct in6_addr))) {
      debug("WG: ip no match\n");
      dump(&allowedip->ip6, sizeof(struct in6_addr));
      dump(&ip->ip6, sizeof(struct in6_addr));
      previp = ip;
      continue;
    }

    debug("WG: found ip\n");

    if (!previp) {
      debug("WG: remove first\n");
      peer->first_allowedip = ip->next_allowedip;
      if (peer->last_allowedip == ip) {
	peer->last_allowedip = NULL;
	peer->first_allowedip = NULL;
      }
    } else {
      debug("WG: remove middle\n");
      // Remove
      if (peer->last_allowedip == ip)
	peer->last_allowedip = previp;

      previp->next_allowedip = ip->next_allowedip;
    }

    free(ip);
    return true;
  }

  return false;
}

static void
wg_rt_notify(struct proto *P, struct channel *CH, struct network *n,
  struct rte *new, struct rte *old UNUSED)
{
  struct wg_proto *p = (struct wg_proto *) P;
  struct wg_config *c = (struct wg_config *) P->cf;
  struct wg_channel *ch = (struct wg_channel *) CH;
  struct iface *iface = NULL;
  const char *ifname = NULL;

//  debug("WG: notify\n");

  if (new) {
    struct nexthop *nh = &new->attrs->nh;
    iface = nh->iface;
    ifname = iface ? iface->name : NULL;

    if (iface && iface == p->iface) {
      struct eattr *t;

      debug("WG: found %p iface %I %p\n", new->attrs, nh->gw, nh->next);

      struct hostentry *he = new->attrs->hostentry;

      debug("WG: he %p src %p\n", he, he?he->src:NULL);
      if (he && he->src) {
	struct eattr *t = ea_find(he->src->eattrs, EA_CODE(PROTOCOL_BGP, BA_TUNNEL_ENCAP));
	debug("WG: he %p %I %I %p %p %I\n", t, he->addr, he->link, he->next, he->src->hostentry, he->src->nh.gw);
      }

      debug("WG: notify new %d %N\n",
        new->attrs->dest, n->n.addr);

      bool is_tunnel_ep = false;
      struct tunnel_encap encap;
      memset(&encap, 0, sizeof(encap));
      encap.ep.ip = IPA_NONE;

      t = ea_find(new->attrs->eattrs, EA_CODE(PROTOCOL_BGP, BA_TUNNEL_ENCAP));
      if (t) {
	log(L_TRACE "WG: Set is tunnel");
	is_tunnel_ep = true;
      }
      if (!t && he && he->src) {
	t = ea_find(he->src->eattrs, EA_CODE(PROTOCOL_BGP, BA_TUNNEL_ENCAP));
      }
      if (t && t->u.ptr && decode_tunnel_encap(t, &encap, P->pool) == 0 && encap.type == c->tunnel_type && encap.encap_len == sizeof(wg_key)) {
	const wg_key *pubkey = encap.encap;
	bool add_ip = true;
	struct wg_entry *en = fib_find(&ch->rtable, n->n.addr);

	if (en) {
	  if (memcpy(en->public_key, pubkey, sizeof(wg_key) == 0)) {
	    add_ip = false;
	  }
	} else {
	  struct wg_entry *en = fib_get(&ch->rtable, n->n.addr);
	  en->is_tunnel_ep = is_tunnel_ep;
	  memcpy(en->public_key, pubkey, sizeof(wg_key));
	}

	log(L_TRACE "WG:     Attr %x %x %d %04x", t->flags, t->type, t->u.ptr->length, encap.flags);

	struct wg_device *dev = p->dev;

	if (dev != NULL) {
	  bool dirty = false;
	  bool found = false;
	  struct wg_peer *peer = NULL;
	  wg_for_each_peer(dev, peer) {
	    // Look for public key
	    size_t len = 32; // FIXME
	    // MIN(32, t->u.ptr->length)
	    if (memcmp(peer->public_key, pubkey, len) != 0) {
	      log(L_TRACE "WG:     Not found");
	      continue;
	    }

	    log(L_TRACE "WG:     Found");
	    found = true;
	    dirty = true;
	    break;
	  }

	  if (!found) {
	    peer = add_peer(dev, pubkey);
	  }

	  dump_peer(peer);
	  if (is_tunnel_ep)
	    set_peer_tunnel_ep(peer, encap.ep.ip, encap.udp_dest_port);
	  if (add_ip)
	    add_allowed_ip(ch->c.net_type, n, peer);
	  dirty = true;

	  if (dirty) {
	    int res = set_device(p);
	    log(L_TRACE "WG: wg_set_device %d", res);
	  }
	}

	
/*
	struct sub_tlv_remove_endpoint {
		u32 asn;
		u8 address_family;
		union {
			ip4_addr ip4;
			ip6_addr ip6;
		};
	};

	struct sub_tlv_udp_dest_port {
		u16 port;
	};

	struct sub_tlv_wireguard {
		u8 pubkey[32];
	};

	struct sub_tlv {
		u8 type;
		union {
			struct {
				u8 length;
				struct sub_tlv_value value[];
			} s8;
			struct {
				u16 length;
				struct sub_tlv_value value[];
			} s16;
		} u;
	};

	struct bgp_tunnel_encap_attr {
		u16 type;
		u16 length;
		struct sub_tlv stlv[];
	};
*/
      } else {
        log(L_TRACE "WG:     No Attr");
      }

      mb_free(encap.encap);
      encap.encap = NULL;

  //  old_metric = en->valid ? en->metric : -1;

//  en->valid = RIP_ENTRY_VALID;
//  en->metric = rt_metric;
//  en->tag = rt_tag;
//  en->from = (new->attrs->src->proto == P) ? new->u.rip.from : NULL;
//  en->iface = new->attrs->iface;
//  en->next_hop = new->attrs->gw;
      }

  } else {
    debug("WG: notify withdraw %N\n",
	  n->n.addr);

    /* Withdraw */
    struct wg_entry *en = fib_find(&ch->rtable, n->n.addr);

    if (!en) { // || en->valid != RIP_ENTRY_VALID)
      debug("WG: fib not found\n");
      return;
    }

	struct wg_device *dev = p->dev;

	if (dev != NULL) {
	  bool marked_peer = false;
	  bool found = false;
	  struct wg_peer *peer = NULL;
	  wg_for_each_peer(dev, peer) {
	    if (en->is_tunnel_ep && !marked_peer) {
	      log(L_TRACE "WG: Is tunnel");
	      if (memcmp(peer->public_key, en->public_key, sizeof(wg_key)) == 0) {
		struct peer_config *pc = NULL;
		bool remove_me = true;
		WALK_LIST(pc,c->peers)
		{
		  wg_key pc_key;
		  if (pc->public_key) {
		    wg_key_from_base64(pc_key, pc->public_key);
		    if (memcmp(pc_key, peer->public_key, sizeof(wg_key)) == 0) {
		      /* Don't remove preconfigured peer */
		      remove_me = false;
		      break;
		    }
		  }
		}

		if (remove_me) {
		  log(L_TRACE "WG: Remove peer");
		  peer->flags |= WGPEER_REMOVE_ME;
		  marked_peer = true;
		  continue;
		}
	      }
	    }
	    // Remove from all peers
	    found = true;

	    if (found) {
	      struct wg_allowedip *allowedip = alloca(sizeof(struct wg_allowedip));
	      init_allowed_ip(allowedip, ch->c.net_type, n);
	      dump_peer(peer);
	      if (remove_allowed_ip(peer, allowedip)) {
		ip_addr ip = allowedip_to_ipa(allowedip);
		log(L_TRACE "WG: removed %I/%d", ip, allowedip->cidr);
		peer->flags |= WGPEER_REPLACE_ALLOWEDIPS;

		dump_peer(peer);
	      }
	    }
	  }

	  if (marked_peer) {
	    remove_marked_peer(p->dev);
	  }
	  int res = set_device(p);
	  log(L_TRACE "WG: wg_set_device %d", res);

	  fib_delete(&ch->rtable, en);
	  en = NULL;
	  /*
    old_metric = en->metric;

    en->valid = RIP_ENTRY_STALE;
    en->metric = p->infinity;
    en->tag = 0;
    en->from = NULL;
    en->iface = NULL;
    en->next_hop = IPA_NONE;
*/
	}
  }

//  TRACE(D_EVENTS, "wg notify %s %s", src_table->name, ifname?ifname:"(null)");
}

static void
wg_reload_routes(struct channel *C)
{
  struct wg_proto *p UNUSED = (struct wg_proto *) C->proto;

  debug("reload routes\n");

// TODO
//  WALK_LIST(c, p->channels)
//    channel_request_feeding(c);
}


static struct proto *
wg_init(struct proto_config *C)
{
  struct wg_config *c UNUSED = (struct wg_config *) C;
  struct proto *P = proto_new(C);
  struct wg_proto *p UNUSED = (struct wg_proto *) P;

  debug("init\n");

  P->if_notify = wg_if_notify;
  P->rt_notify = wg_rt_notify;
  P->reload_routes = wg_reload_routes;
//  P->accept_ra_types = RA_ANY;

  /* Add all channels */
  struct wg_channel_config *cc;
  WALK_LIST(cc, C->channels)
    proto_add_channel(P, &cc->c);

  return P;
}


static int
wg_start(struct proto *P)
{
  struct wg_config *cf UNUSED = (struct wg_config *) P->cf;
  struct wg_proto *p = (struct wg_proto *) P;

  log(L_TRACE "WG: start");

  if (get_device(p, &p->dev, cf->ifname) >= 0)
    {
      int res = set_device(p);
      log(L_TRACE "WG: wg_set_device %d", res);
    }

  struct wg_channel *ch;
  WALK_LIST(ch,p->p.channels) {
    fib_init(&ch->rtable, P->pool, ch->c.net_type, sizeof(struct wg_entry),
	     OFFSETOF(struct wg_entry, n), 0, wg_init_entry);
  }
  return PS_UP;
}

static int
wg_shutdown(struct proto *P)
{
  struct wg_config *cf =  (struct wg_config*)P->cf;
  struct wg_proto *p = (struct wg_proto*)P;
  wg_device *dev = NULL;

  log(L_TRACE "WG: wg_shutdown");
  if (get_device(p, &p->dev, cf->ifname) >= 0)
    {
      int res = set_device(p);
      log(L_TRACE "WG: flush wg_set_device %d", res);
    }

  return PS_DOWN;
}

static void
wg_dump(struct proto *P)
{
  struct wg_proto *p = (struct wg_proto *) P;
  int i;

  i = 0;

  struct wg_channel *ch;
  WALK_LIST(ch,p->p.channels) {
    FIB_WALK(&ch->rtable, struct wg_entry, en)
      {
	//    struct wg_entry *en = (struct wg_entry *) e;
	debug("WG: entry #%d:\n",
	      i++);
      }
    FIB_WALK_END;
  }

  struct wg_peer *peer = NULL;

  log(L_TRACE "WG: dump peers");
  wg_for_each_peer(p->dev, peer) {
    dump_peer(peer);
  }
}

static void
wg_copy_config(struct proto_config *DEST, struct proto_config *SRC)
{
  struct wg_config *dest = (struct wg_config *)DEST;
  struct wg_config *src = (struct wg_config *)SRC;

  dest->ifname = src->ifname;
  dest->socket_path = src->socket_path;
  dest->private_key = src->private_key;
  dest->listen_port = src->listen_port;

  struct peer_config *spc = NULL;
  WALK_LIST(spc,src->peers)
    {
      struct peer_config *dpc = cfg_allocz(sizeof(struct peer_config));
      dpc->public_key = spc->public_key;
      dpc->listen_port = spc->listen_port;
      dpc->endpoint = spc->endpoint;
      dpc->remote_port = spc->remote_port;
      dpc->allowedip = spc->allowedip;
      add_tail(&dest->peers, (node*)spc);
    }
}

struct peer_config *peer_new(struct wg_config *c)
{
  struct peer_config *pc = cfg_allocz(sizeof(struct peer_config));
  debug("peer_new %p\n", pc);
  add_tail(&c->peers, (node*)pc);
  return pc;
}


static void
wg_channel_init(struct channel *CH, struct channel_config *CHC)
{
  struct wg_channel *ch = (struct wg_channel*)CH;
  struct proto *P = CH->proto;

  /* Create new instance */
  log(L_TRACE "WG: wg_channel_init");
}

static int
wg_channel_reconfigure(struct channel *CH, struct channel_config *CHC,
		       int *import_changed, int *export_changed)
{
  /* Try to reconfigure instance, returns success */
  log(L_TRACE "WG: wg_channel_reconfigure");
  return 1;
}

static int
wg_channel_start(struct channel *CH)
{
  struct wg_channel *ch = (struct wg_channel*)CH;
  struct proto *P = CH->proto;

  /* Start the instance */
  log(L_TRACE "WG: wg_channel_start");
#if 0
  fib_init(&ch->rtable, P->pool, ch->c.net_type, sizeof(struct wg_entry),
	   OFFSETOF(struct wg_entry, n), 0, wg_init_entry);
#endif
  return 1;
}

static void
wg_channel_shutdown(struct channel *CH)
{
  struct wg_channel *ch = (struct wg_channel*)CH;

  /* Stop the instance */
  log(L_TRACE "WG: wg_channel_shutdown");
}

static void
wg_channel_cleanup(struct channel *CH)
{
  struct wg_channel *ch = (struct wg_channel*)CH;

  /* Channel finished flush */
  log(L_TRACE "WG: wg_channel_cleanup");
}


struct channel_class channel_wg = {
  .channel_size =       sizeof(struct wg_channel),
  .config_size =        sizeof(struct wg_channel_config),
  .init =               wg_channel_init,
  .start =              wg_channel_start,
  .shutdown =           wg_channel_shutdown,
  .cleanup =            wg_channel_cleanup,
  .reconfigure =        wg_channel_reconfigure,
};

struct protocol proto_wireguard = {
  .name =		"Wireguard",
  .template =		"wg%d",
  .class =              PROTOCOL_WG,
  .channel_mask =       NB_IP,
  .proto_size =         sizeof(struct wg_proto),
  .config_size =	sizeof(struct wg_config),
  .postconfig =		wg_postconfig,
  .init =		wg_init,
  .start =		wg_start,
  .shutdown =		wg_shutdown,
  .dump = 		wg_dump,
  .copy_config = 	wg_copy_config,
/*  .multitable =		1,
  .preference =		DEF_PREF_PIPE,
  .cleanup =		wg_cleanup,
  .reconfigure =	wg_reconfigure,
  .copy_config = 	wg_copy_config,
  .get_status = 	wg_get_status,
  .show_proto_info = 	wg_show_proto_info*/
};