WIP set allowed-ips

1 files changed, 140 insertions, 5 deletions

diff --git a/proto/wireguard/wireguard.c b/proto/wireguard/wireguard.c
index f45796d5..85d6d10b 100644
--- a/proto/wireguard/wireguard.c
+++ b/proto/wireguard/wireguard.c
@@ -35,10 +35,11 @@ wg_if_notify(struct proto *P, unsigned flags, struct iface *i)
 }
 
 static void
-wg_rt_notify(struct proto *P, rtable *src_table, net *n,
+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;
@@ -59,8 +60,55 @@ wg_rt_notify(struct proto *P, rtable *src_table, net *n,
         new->attrs->dest, n->n.addr);
 
       t = ea_find(new->attrs->eattrs, EA_CODE(PROTOCOL_BGP, BA_TUNNEL_ENCAP));
-      if (t) {
-        log(L_TRACE "WG:     Attr %x %x", t->flags, t->type);
+      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;
@@ -114,7 +162,8 @@ wg_rt_notify(struct proto *P, rtable *src_table, net *n,
       }
 
   } else {
-    debug("WG: notify withdraw\n");
+    debug("WG: notify withdraw %N\n",
+	  n->n.addr);
 
     /* Withdraw */
     en = fib_find(&p->rtable, n->n.addr);
@@ -122,7 +171,92 @@ wg_rt_notify(struct proto *P, rtable *src_table, net *n,
     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;
@@ -132,6 +266,7 @@ wg_rt_notify(struct proto *P, rtable *src_table, net *n,
     en->iface = NULL;
     en->next_hop = IPA_NONE;
 */
+	}
   }
 
 //  TRACE(D_EVENTS, "wg notify %s %s", src_table->name, ifname?ifname:"(null)");