path: root/proto/wireguard/wireguard.c

// Based on proto/rip/rip.c

#include <stdio.h>
#include "nest/protocol.h"
#include "nest/iface.h"
#include "sysdep/linux/wireguard.h"
#include "wireguard.h"
#include "proto/bgp/bgp.h"

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

static void
wg_postconfig(struct proto_config *C)
{
  C;
  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 = 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;
  }
}

static void
wg_rt_notify(struct proto *P, struct channel *ch, net *n,
  rte *new, rte *old, ea_list *attrs)
{
  struct wg_proto *p = (struct wg_proto *) P;
  struct wg_config *c = P->cf;
  struct wg_entry *en;
  struct iface *iface = NULL;
  const char *ifname = NULL;

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

  if (new) {
    iface = new->attrs->nh.iface;
    ifname = iface ? iface->name : NULL;

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

      debug("WG: found iface\n");
      en = fib_get(&p->rtable, n->n.addr);

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

      t = ea_find(new->attrs->eattrs, EA_CODE(PROTOCOL_BGP, BA_TUNNEL_ENCAP));
      if (t && t->u.ptr) {
	log(L_TRACE "WG:     Attr %x %x %d", t->flags, t->type, t->u.ptr->length);

	struct wg_device *dev = NULL;

	if (wg_get_device(&dev, ifname) == 0) {
	  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, &t->u.ptr->data[t->u.ptr->length - 32], len) != 0) {
	      log(L_TRACE "WG:     Not found");
	      continue;
	    }

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

	    if (found) {
	      // Add allowed ip
	      struct wg_allowedip *allowedip = malloc(sizeof(struct wg_allowedip));
	      memset(allowedip, 0, sizeof(struct wg_allowedip));

	      if (p->p.cf->net_type == NET_IP4) {
		allowedip->family = AF_INET;
		allowedip->ip4.s_addr = ip4_to_u32(ip4_hton(net4_prefix(n->n.addr)));
	      } else if (p->p.cf->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);
	      peer->last_allowedip->next_allowedip = allowedip;
	      peer->last_allowedip = allowedip;

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

	  wg_free_device(dev);
	  dev = NULL;
	}

	
/*
	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");
      }

  //  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 */
    en = fib_find(&p->rtable, n->n.addr);

    if (!en) // || en->valid != RIP_ENTRY_VALID)
      return;

	struct wg_device *dev = NULL;

	if (wg_get_device(&dev, c->ifname) == 0) {
	  bool found = false;
	  struct wg_peer *peer = NULL;
	  wg_for_each_peer(dev, peer) {
	    // Look for public key
	    found = true;

	    if (found) {
	      // Add allowed ip
	      struct wg_allowedip *allowedip = malloc(sizeof(struct wg_allowedip));
	      memset(allowedip, 0, sizeof(struct wg_allowedip));

	      if (p->p.cf->net_type == NET_IP4) {
		allowedip->family = AF_INET;
		allowedip->ip4.s_addr = ip4_to_u32(ip4_hton(net4_prefix(n->n.addr)));
	      } else if (p->p.cf->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);

	      struct wg_allowedip *ip = NULL;
	      struct wg_allowedip *previp = NULL;
	      
	      wg_for_each_allowedip(peer, ip) {
		if (allowedip->family != ip->family) {
		  previp = ip;
		  continue;
		}

		if (allowedip->cidr != ip->cidr) {
		  previp = ip;
		  continue;
		}
		
		if (memcmp(&allowedip->ip6, &ip->ip6, sizeof(struct in6_addr))) {
		  previp = ip;
		  continue;
		}

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

		struct wg_allowedip *next = ip->next_allowedip;

		if (peer->first_allowedip == ip) {
		  debug("WG: remove first\n");
		  peer->first_allowedip = ip->next_allowedip;
		  if (peer->last_allowedip == ip)
		    peer->last_allowedip = NULL;
		} else {
		  debug("WG: remove middle\n");
		  // Remove
		  struct wg_allowedip *cur = NULL;
		  wg_for_each_allowedip(peer, cur) {
		    if (cur->next_allowedip != ip)
		      continue;

		    debug("WG: remove next\n");
		    if (peer->last_allowedip == ip)
		      peer->last_allowedip = cur;

		    cur->next_allowedip = ip->next_allowedip;
		    break;
		  }
		}

		free(ip);
		break;
		/* ip = next; */
	      }
	      
	      peer->flags |= WGPEER_REPLACE_ALLOWEDIPS;
	      int res = wg_set_device(dev);
	      log(L_TRACE "WG: wg_set_device %d", res);
	      break;
	    }
	  }

	  wg_free_device(dev);
	  dev = 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 int
wg_reload_routes(struct proto *P)
{
  struct wg_proto *p = (struct wg_proto *) P;

  debug("reload routes\n");

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

 return 1;
}


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

  debug("init\n");

  P->main_channel = proto_add_channel(P, proto_cf_main_channel(C));

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

  return P;
}


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

  debug("start\n");
  fib_init(&p->rtable, P->pool, P->net_type, sizeof(struct wg_entry),
	 OFFSETOF(struct wg_entry, n), 0, wg_init_entry);
  return PS_UP;
}

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

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


struct protocol proto_wireguard = {
  .name =		"Wireguard",
  .template =		"wg%d",
  .class =              PROTOCOL_WG,
  .channel_mask =       NB_ANY,
  .proto_size =         sizeof(struct wg_proto),
  .config_size =	sizeof(struct wg_config),
  .postconfig =		wg_postconfig,
  .init =		wg_init,
  .start =		wg_start,
  .dump = 		wg_dump,
/*  .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*/
};