summaryrefslogblamecommitdiffstats
path: root/src/if_wg.c
blob: fe2eb0d99d06303b98d060f5cc8d2f3d7f68ecbc (plain) (tree)
1
2
  
                                                        














                                                                           


                      
                       



                       

                          

                       
                        
                    
                         
                            

                   
                       

                         

                          


                       



                            




                     
               



                                        
                           
                                       


                                  
                                         

  






                                                          

  



                                                 

  
                 





                                          
            

                                           
      








                                                  

  
                          

                                   
 


                                                                                

                                                                           
  
 




                                                                             
 


                                                            
             
                                                                                            
                                                                            
                                                                               
 
                                                                     
                                                
 

                                                    

                                                                               
 
                                              
                                           

                                              

                                          

                                                                            
                                                                         
                                                 
                                                  

                               



                                                         



                                                             



                                                                            
                                                                 
                                                         

                                                        
 
   
                                                                                 
 

                              
                                     
 
                                                                             
                              


                                       



                                                                        
                                              
                              
         
 


                                                           

                                                
 
                                      



                 
                                                                
 
                           

                              
                                     
 
                                                                             



                                       

                                                                                
                                              
                               

         



                                                                      

                                                                           

                                                       
 
                                      
 

                 
 
                 
                                                                      
 



                               

                              
                                  
 


                                             
                        
                                                                            
                    
                                                                            
                      
            

                                                   
                        
                                                                               
                    
                                                                               
                      
      



                            
                         
                                                                   
 
                       
 
                 

 
   
                                                      
 



                                       

                                                




                                              


                                                       
                                                                              
                                                   

                          
                                    











                                                                      
                                                                               



                                                                   
                                                           




                                                         
                                                         










                                                                          
                                                  





                                                                               
 

                                                       
 
                                                                            







                                                                      
                                   



                                                                             
                 
                                







                                                                             

 
               
                               














                                                                      
                                                         
    
                                            
 
                                                              
 
 
    
                                   
 
                                                                 
                                 
                                                              
 
 
    
                                           
 






                                                                         


    
                                  
 
                                                           
                                 

 
    
                                                                           
 


                                          
 

                                                      
 
                       
                         
                           
                                        
 
                                    

                        
                                       







                                                             


    
                                                                        
 
                                               




                             

                                   
                                                
 


                                                                    

                               




                                                                      

                             




                                                                    




                                                                
                                                                            
                                                        
 
                                  
                                         

                                                      
               
     
                                                                         
                               
                                         

 

                          
 

                          
                        

                                   
                                     
 

                                                        

                                                            
                                                                           
 



                                                                    
 
                                                
                                   

                              
                                                  
 
                                               

                                                  


                                                                             
 
                            


                                                                        
                
                                                                               

         

                                                                       
 
                                   
                                         

                                                      
               
     
                    
                                                                
                                                                           
                                         
 
 
    
                             
 
                                  
 

                                   
 
                                 
                            
 
                                             
                                                                  
 

                               

                                                    
 


                                                                             
 

                             

                                                    
 


                                                                           
 
                                                                   

                           


                                                                         

                      
                                                                 
         
 
                                                                            
                                                        






                                                                
                                                                                             
                                         

 

                          
 

                                   

                                                                          
 

                            

                                                  
 


                                                                  
 
                                                  
 
                                                      
                                                 
                                                                               
                               

         






                                                                            

                                             
 

                                                                           

                                                                                 
 
                                                                            
                             
                                                            
 

                                                              


                                           
                                         






                                                                       
               

                                                                
                                                                           
                                         

 




                                                                            
 


                                            

                                                                                      
                                                


   

                                                                 
 
                  
                           
                               
                                   

                                            
                                           




                                                                 

                                                                 



                               

                                              
                                                   
 
                                                                     



                                                                             

                                                         






                                                                 





                                                                       
                                                                    





                                                                         





                                         
                                                
                                                                 
                              

                             
 
                 


             
                                                                  
 
                           









                                                    
                                               
                          
 
                                              
                       

                                                                       




                               
                                                      


                                                             
                                                 




                                                        
 
 







                                                         
    
                                

                                                        
                                                

                                        
                                             
                                                      

                                                   
                                                     




                                                                            







                                                                                 
                                                           
         


    
                                 
 
                  
                          
                            
                                 
                                                
 
                                             
                                                      

                                                   
                                                     
                                           


                                                     

                                                      
 


                                                                            
                         
                                           
                                                                              

                                                                           
                                        
                        
                                    
                 


                                   
                
                                                           
         

 
                                                       

                                                              

    
                 
 
                                    

                                                              

 

                                                
 
                             


                                                                               
                   
                                                      
                                           
                                    
                                   

                                 


                                                       
                                    
                                   
                                 
      
 
                                                   
 
                                                                        
                                                                   
                                                                                  

                                                                                  
                                            
                                                 

                                                 
 
                   
 





                                                                     
                                                                              
                                     

                                 
                                   
                                 






                                                 
                               

                 
                                                                 

      



                         
                                                              
 







                                            

                                             
                                 

                                    




                               
 




                       
 
                                                                
 
                                                            
            
                                                              
      
                                        
 


                   

                                 
 



                                
            
                                  
      
 
                            
 
                                                   
 
                                   
                                   


                                                
 





                                               
 

                                                                 
 


                                                     
                                   



                                                                 





                                                                 
 

                                                                 
      
 
                 

 
   

                                                                           
                    





                                            



                             
                                                                    

                                                                  

                                                                         

















                                                                            

                                                            


                                    


                           
                                         

                            

                                                                           


                                                                
                      
                            
                                                                                
                      
                         
                                                              




                                                                       
                      
                            
                                                                  
                      
                            
                                                              
                      
                            

                                                                       
                      
         
 

                          
                   


   
                                                                           
 
                             
                             
                               
 

                             

                      
                                                             
                                           
                                        
                                                                            
                                                                      




                                                                   
                                       
                 

                                                   
                                      
 
                                                     
                      
                                                          







                                                               
                       


                                   

                                   

                                                                  

                                                          

                                                                             
            
                                                                       
 


                              
                                                                         
                                                      
                                              

                                                                                   









                                                               



                                 
 


                                                                      

                              

                            
                                                            


                                                                   
            
                                                               
 

                                                                         
 


                                            
 

                                                  
                                          
                                                                                     


                              
                          









                                                      





                                                                              
                      


                                                                   
                                                                          


                                                                  






                                                                              

                      
                              




                                                   


                                                                     
                        

                                                                     
                         

                      
                        

                                                             
                                     


                                                   


                                                          
                                                         
                                     
                        

                                                             
            

                                                             
      

                                               

                               
                                                                         

                      




                          
                             

         
                   
 
/*
 * Copyright (c) 2019 Matt Dunwoodie <ncon@noconroy.net>
 *
 * Permission to use, copy, modify, and distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 */

#include <sys/types.h>
#include <sys/systm.h>
#include <sys/param.h>

#include <sys/socket.h>
#include <sys/ioctl.h>
#include <sys/mbuf.h>

#include <sys/sockio.h>
#include <sys/socketvar.h>
#include <sys/errno.h>
#include <sys/proc.h>
#include <sys/rwlock.h>
#include <sys/protosw.h>
#include <sys/mpq.h>
#include <sys/fixedmap.h>
#include <sys/bloombucket.h>

#include <net/if.h>
#include <net/if_var.h>
#include <net/if_types.h>
#include <net/pfvar.h>
#include <net/wireguard.h>
#include <net/if_wg.h>

#include <netinet/in.h>
#include <netinet/ip.h>
#include <netinet/ip_icmp.h>
#include <netinet/icmp6.h>
#include <netinet/in_pcb.h>

#include "bpfilter.h"
#if NBPFILTER > 0
#include <net/bpf.h>
#endif

struct wg_tag {
	union wg_ip		 t_ip;
	struct wg_softc		*t_sc;
	uint32_t		 t_dst;
	enum wg_pkt_type	 t_type;
	enum wg_pkt_state {
		WG_PKT_STATE_EMPTY = 0,
		WG_PKT_STATE_NEW,
		WG_PKT_STATE_DONE,
		WG_PKT_STATE_DEAD,
	}			 t_state;
};

struct wg_route {
	union wg_ip		 	 r_ip;
	struct wg_peer	 	 	*r_peer;
	/* TODO do we need r_sc? not used anywhere else */
	struct wg_softc			*r_sc;
	struct mbuf_queue	 	 r_outgoing;
	SLIST_HEAD(, wg_route_entry)	 r_aip;
};

struct wg_route_entry {
	SLIST_ENTRY(wg_route_entry)	 r_entry;
	struct wg_cidr		 	 r_cidr;
	struct wg_route			*r_dst;
};

struct wg_softc {
	SLIST_ENTRY(wg_softc)	 sc_next;
	struct ifnet 		 sc_if;
	in_port_t		 sc_port;
	struct wg_device	 sc_dev;
	struct art_root		*sc_ip_rt;
	struct socket		*sc_so4;
#ifdef INET6
	struct art_root		*sc_ip6_rt;
	struct socket		*sc_so6;
#endif
	struct taskq		*sc_taskq;
	struct task		 sc_tx_task;
	struct task		 sc_tx_slow_task;
	struct task		 sc_rx_task;
	struct task		 sc_rx_slow_task;
	struct mpq 		 sc_tx_queue;
	struct mpq 		 sc_tx_slow_queue;
	struct mpq 		 sc_rx_queue;
	struct mpq 		 sc_rx_slow_queue;
};

struct bloom_bucket wg_bb;
struct bloom_bucket wg_bb_verified;


#define DPRINTF(sc, str, ...) do { if (ISSET((sc)->sc_if.if_flags, IFF_DEBUG)) \
    printf("%s: " str, (sc)->sc_if.if_xname, __VA_ARGS__); } while (0)
/*
#define DPRINTF(sc, ...) do { if (ISSET((sc)->sc_if.if_flags, IFF_DEBUG)) \
    printf("wg: " __VA_ARGS__); } while (0)
*/

#ifdef INET6
#define AF_VAL(af, v4, v6) ((af) == AF_INET ? v4 : (af) == AF_INET6 ? v6 : 0)
#else
#define AF_VAL(af, v4, _) ((af) == AF_INET ? v4 : 0)
#endif

#define drop_pkt_err(e) do { err = e; goto drop; } while (0)
#define peer_route(p) ((struct wg_route *)(p)->p_arg)

/* if_wg.c */
int		 wg_softc_route_add(struct wg_softc *, struct wg_cidr *, struct wg_route *);
int		 wg_softc_route_delete(struct wg_softc *, struct wg_cidr *);
struct wg_route	*wg_softc_route_lookup(struct wg_softc *, struct mbuf *, bool);

int		 wg_mbuf_ratelimit(struct wg_softc *, struct mbuf *);
struct wg_tag	*wg_mbuf_get_tag(struct mbuf *);

void		 wg_route_init(struct wg_route *);
void		 wg_route_broken(struct wg_route *);
void		 wg_route_queue(struct wg_route *, enum wg_pkt_type, uint32_t);
void		 wg_peer_queue(struct wg_peer *, enum wg_pkt_type, uint32_t);

void		 wg_encrypt_hs(struct mbuf *);
void		 wg_encrypt(struct mbuf *);
void		 wg_decrypt_hs(struct mbuf *);
void		 wg_decrypt(struct mbuf *);

void		 wg_start(struct ifnet *);
int		 wg_output(struct ifnet *, struct mbuf *, struct sockaddr *,
		    struct rtentry *);
struct mbuf	*wg_input(void *, struct mbuf *, struct sockaddr *, int);
void		 wg_input_deliver(struct mbuf *);
void		 wg_output_deliver(struct mbuf *);

void		 wgattach(int);
int		 wg_clone_create(struct if_clone *, int);
int		 wg_clone_destroy(struct ifnet *);
int		 wg_bind_port(struct wg_softc *);

int		 wg_ioctl_set_peer(struct wg_softc *, u_long,
		    struct wg_set_peer *);
int		 wg_ioctl_set_serv(struct wg_softc *, u_long,
		    struct wg_set_serv *);
void		 wg_ioctl_get_serv(struct wg_softc *, struct wg_get_serv *);
int		 wg_ioctl_get_peer(struct wg_softc *, struct wg_get_peer *);
int		 wg_ioctl(struct ifnet *, u_long, caddr_t);

MPQ_WORKER(wg_tx_slow_task_fn, wg_encrypt_hs, wg_output_deliver);
MPQ_WORKER(wg_tx_task_fn, wg_encrypt, wg_output_deliver);
MPQ_WORKER(wg_rx_slow_task_fn, wg_decrypt_hs, m_freem);
MPQ_WORKER(wg_rx_task_fn, wg_decrypt, wg_input_deliver);

int
wg_softc_route_add(struct wg_softc *sc, struct wg_cidr *cidr, struct wg_route *r)
{
	struct art_node	*node;
	struct art_root	*root;
	struct wg_route_entry *route;

	if ((root = AF_VAL(cidr->c_af, sc->sc_ip_rt, sc->sc_ip6_rt)) == NULL)
		return EINVAL;

	rw_enter_write(&root->ar_lock);

	node = malloc(sizeof(*node), M_DEVBUF, M_WAITOK | M_ZERO);

	if (art_insert(root, node, &cidr->c_ip, cidr->c_mask) != node) {
		free(node, M_DEVBUF, sizeof(*node));
		rw_exit_write(&root->ar_lock);
		return EEXIST;
	}

	route = malloc(sizeof(*route), M_DEVBUF, M_WAITOK);
	SLIST_INSERT_HEAD(&r->r_aip, route, r_entry);
	route->r_dst = r;
	route->r_cidr = *cidr;
	node->an_gc = (struct art_node *) route;

	rw_exit_write(&root->ar_lock);
	return 0;
}

int
wg_softc_route_delete(struct wg_softc *sc, struct wg_cidr *cidr)
{
	struct srp_ref	sr;
	struct art_node	*node;
	struct art_root	*root;
	struct wg_route_entry *route;

	if ((root = AF_VAL(cidr->c_af, sc->sc_ip_rt, sc->sc_ip6_rt)) == NULL)
		return EINVAL;

	rw_enter_write(&root->ar_lock);

	if ((node = art_lookup(root, &cidr->c_ip, cidr->c_mask, &sr)) == NULL) {
		srp_leave(&sr);
		rw_exit_write(&root->ar_lock);
		return ENOATTR;
	}

	if (art_delete(root, node, &cidr->c_ip, cidr->c_mask) == NULL)
		panic("art_delete failed to delete node %p", node);

	srp_leave(&sr);
	route = (struct wg_route_entry *) node->an_gc;
	SLIST_REMOVE(&route->r_dst->r_aip, route, wg_route_entry, r_entry);
	free(route, M_DEVBUF, sizeof(struct wg_route));
	free(node, M_DEVBUF, sizeof(*node));

	rw_exit_write(&root->ar_lock);

	return 0;
}

struct wg_route *
wg_softc_route_lookup(struct wg_softc * sc, struct mbuf * m, bool out)
{
	struct ip *iphdr;
#ifdef INET6
	struct ip6_hdr *ip6hdr;
#endif
	struct art_node	*node;
	struct srp_ref	sr;
	struct wg_route	*r = NULL;

	switch (m->m_pkthdr.ph_family) {
	case AF_INET:
		iphdr = mtod(m, struct ip *);
		if (out)
			node = art_match(sc->sc_ip_rt, &iphdr->ip_dst, &sr);
		else
			node = art_match(sc->sc_ip_rt, &iphdr->ip_src, &sr);
		break;
#ifdef INET6
	case AF_INET6:
		ip6hdr = mtod(m, struct ip6_hdr *);
		if (out)
			node = art_match(sc->sc_ip6_rt, &ip6hdr->ip6_dst, &sr);
		else
			node = art_match(sc->sc_ip6_rt, &ip6hdr->ip6_src, &sr);
		break;
#endif
	default:
		return NULL;
	}

	if (node != NULL)
		r = ((struct wg_route_entry *) node->an_gc)->r_dst;

	srp_leave(&sr);

	return r;
}

int
wg_mbuf_ratelimit(struct wg_softc *sc, struct mbuf *m)
{
	enum wg_error e;
	struct wg_cookie c;
	struct wg_msg_response *resp;
	struct wg_msg_initiation *init;
	struct wg_tag *tag = wg_mbuf_get_tag(m);
	union wg_ip *ip = &tag->t_ip;

	uint8_t *mac, *token, token_len;
	uint32_t sender;

	/* Get token from source IP address */
	token = AF_VAL(ip->sa.sa_family,
	    (uint8_t *) &satosin(&ip->sa)->sin_addr,
	    (uint8_t *) &satosin6(&ip->sa)->sin6_addr);
	/* Use upper 8 octets for INET6, so we filter based on a /64 subnet */
	token_len = AF_VAL(ip->sa.sa_family, 4, 8);

	if (token == NULL)
		panic("invalid af");

	/* If we don't want to rate limit, return OK */
	if (!bb_recv(&wg_bb, token, token_len))
		return 0;

	/*
	 * From here on, the peer has been potentially sending many
	 * packets. We need to verify if they are just spoofing IP src
	 * addresses, or they received a false positive from wg_bb.
	 */

	/* Calculate cookie and mac from packet */
	wg_cookie_from_token(&c, &sc->sc_dev.d_cookie_maker, token, token_len);

	switch (wg_pkt_type(mtod(m, uint8_t *), m->m_pkthdr.len)) {
	case WG_PKT_INITIATION:
		init = mtod(m, struct wg_msg_initiation *);
		e = wg_msg_initiation_valid_mac2(init, &c);
		sender = init->sender;
		mac = init->mac1;
		break;
	case WG_PKT_RESPONSE:
		resp = mtod(m, struct wg_msg_response *);
		e = wg_msg_response_valid_mac2(resp, &c);
		sender = resp->sender;
		mac = resp->mac1;
		break;
	default:
		panic("only ratelimit initiation and response");
	}

	/* If mac is invalid, and wg_bb is under high load, it is likely a
	 * bruteforce attack with a spoofed source address. We can use the
	 * cookie to validate the source address. TODO calcluate a good
	 * default value for high load, rather than just 10. */
	if (bb_load(&wg_bb) > 10 && e == WG_MAC) {
		int error;
		struct socket *so;
		struct mbuf peernam;
		struct mbuf *m = m_clget(NULL, M_WAITOK,
		    sizeof(struct wg_msg_cookie));
		struct wg_msg_cookie *cookie = mtod(m, struct wg_msg_cookie *);

		/* TODO print ip */
		DPRINTF(sc, "transmit cookie %d\n", 0);

		wg_device_make_cookie(&sc->sc_dev, &c, sender, mac, cookie);

		bzero(&peernam, sizeof(struct mbuf));

		peernam.m_type = MT_SONAME;
		peernam.m_data = (caddr_t) ip;
		peernam.m_len = ip->sa.sa_len;

		so = AF_VAL(ip->sa.sa_family, sc->sc_so4, sc->sc_so6);
		int s = solock(so);
		if (so) {
			if ((error = so->so_proto->pr_usrreq(so, PRU_SEND, m,
			    &peernam, NULL, NULL)) != 0)
				DPRINTF(sc, "unable to send: %d\n", error);
		}
		sounlock(so, s);
	}

	/* If we get to here, we either have a valid packet, or we are under
	 * an attack from a coordinated bruteforce attack, where the attacker
	 * has control of all the source addresses. For the time being, we
	 * will just accept this and try to process the packet. */

	return 0;
}

struct wg_tag *
wg_mbuf_get_tag(struct mbuf *m)
{
	struct m_tag	*mtag;

	if ((mtag = m_tag_find(m, PACKET_TAG_TUNNEL, NULL)) == NULL) {
		mtag = m_tag_get(PACKET_TAG_TUNNEL,
		    sizeof(struct wg_tag), M_NOWAIT);
		if (mtag == NULL)
			return (NULL);
		bzero(mtag + 1, sizeof(struct wg_tag));
		m_tag_prepend(m, mtag);
	}

	return ((struct wg_tag *)(mtag + 1));
}

/* The following functions are the peer send functions */
void
wg_route_send_initiation(struct wg_route *r)
{
	wg_route_queue(r, WG_PKT_INITIATION, r->r_peer->p_id);
}

void
wg_route_broken(struct wg_route *r)
{
	DPRINTF(r->r_sc, "broken session %x\n", r->r_peer->p_id);
	wg_peer_clean(r->r_peer);
	wg_route_queue(r, WG_PKT_INITIATION, r->r_peer->p_id);
}

void
wg_route_send_keepalive(struct wg_route *r)
{
	struct wg_session *session;
	if ((session = wg_peer_ks_session(r->r_peer)) == NULL) {
		wg_route_send_initiation(r);
	} else {
		wg_route_queue(r, WG_PKT_TRANSPORT, session->s_local_id);
		wg_session_put(session);
	}
}

void
wg_route_clean(struct wg_route *r)
{
	DPRINTF(r->r_sc, "cleaning %x\n", r->r_peer->p_id);
	wg_peer_clean(r->r_peer);
}

void
wg_route_queue(struct wg_route *route, enum wg_pkt_type type, uint32_t dst)
{
	struct mbuf *m;
	struct wg_tag *tag;
	struct wg_softc *sc = route->r_sc;

	m = m_clget(NULL, M_WAITOK, wg_pkt_len[type]);
 	tag = wg_mbuf_get_tag(m);

	tag->t_sc = sc;
	tag->t_dst = dst;
	tag->t_type = type;
	tag->t_state = WG_PKT_STATE_NEW;

	m->m_len = wg_pkt_len[type];
	m_calchdrlen(m);

	if (type == WG_PKT_TRANSPORT) {
		m->m_len = 0;
		m_calchdrlen(m);
		mpq_enqueue(&sc->sc_tx_queue, m);
		task_add(sc->sc_taskq, &sc->sc_tx_task);
	} else {
		mpq_enqueue(&sc->sc_tx_slow_queue, m);
		task_add(sc->sc_taskq, &sc->sc_tx_slow_task);
	}
}

void
wg_peer_queue(struct wg_peer *peer, enum wg_pkt_type type, uint32_t dst)
{
	wg_route_queue(peer->p_arg, type, dst);
}

void
wg_encrypt_hs(struct mbuf *m)
{
	enum wg_error err;
	struct wg_session *session;
	struct wg_tag *tag = wg_mbuf_get_tag(m);

	if (tag->t_state != WG_PKT_STATE_NEW)
		panic("not a new state packet: %d\n", tag->t_state);

	switch (tag->t_type) {
	case WG_PKT_INITIATION:
		if ((err = wg_device_tx_initiation(&tag->t_sc->sc_dev,
		    mtod(m, struct wg_msg_initiation *), tag->t_dst,
		    &session)) != WG_OK)
			drop_pkt_err(err);
		wg_timer_reinit_flag(&session->s_peer->p_timers);
		break;
	case WG_PKT_RESPONSE:
		if ((err = wg_device_tx_response(&tag->t_sc->sc_dev,
		    mtod(m, struct wg_msg_response *), tag->t_dst,
		    &session)) != WG_OK)
			drop_pkt_err(err);
		wg_timer_reinit_unflag(&session->s_peer->p_timers);
		break;
	default:
		panic("invalid packet type: %d\n", tag->t_type);
	}

	DPRINTF(tag->t_sc, "tx %s for %x, id %x\n", wg_pkt_str[tag->t_type],
	    session->s_peer->p_id, session->s_local_id);

	m->m_pkthdr.ph_cookie = m;
	tag->t_state = WG_PKT_STATE_DONE;
	tag->t_ip = peer_route(session->s_peer)->r_ip;
	wg_session_put(session);
	return;
drop:
	DPRINTF(tag->t_sc, "%s tx failed: %s\n", wg_pkt_str[tag->t_type],
	    wg_error_str[err]);
	tag->t_state = WG_PKT_STATE_DEAD;
}

void
wg_encrypt(struct mbuf *m)
{
	enum wg_error err;

	struct mbuf *em;
	struct wg_tag *tag;
	struct wg_session *session;
	struct wg_msg_transport *msg;

	size_t plain_len = m->m_pkthdr.len;
	size_t padding_len = WG_PADDING_SIZE(plain_len);
	size_t clear_len = plain_len + padding_len;
	size_t encrypted_len = WG_ENCRYPTED_SIZE(clear_len);
	size_t total_len = encrypted_len + sizeof(struct wg_msg_transport);

	tag = wg_mbuf_get_tag(m);

	if (tag->t_state != WG_PKT_STATE_NEW)
		panic("not a new state packet: %d\n", tag->t_state);

	em = m_clget(NULL, M_WAITOK, total_len);
	m->m_pkthdr.ph_cookie = em;
	em->m_len = total_len;
	m_calchdrlen(em);
	msg = mtod(em, struct wg_msg_transport *);

	m_copydata(m, 0, plain_len, msg->data);
	bzero(msg->data + plain_len, padding_len);

	if ((err = wg_device_tx_transport(&tag->t_sc->sc_dev, msg, clear_len,
	    tag->t_dst, &session)) != WG_OK)
		goto drop;

	if (plain_len > 0) {
		counters_pkt(tag->t_sc->sc_if.if_counters, ifc_opackets,
		    ifc_obytes, plain_len);
		wg_timer_broken_flag(&session->s_peer->p_timers);
	} else {
		DPRINTF(tag->t_sc, "send keepalive %x\n", session->s_local_id);
	}

	wg_timer_keepalive_unflag(&session->s_peer->p_timers);
	wg_timer_persistent_keepalive_tick(&session->s_peer->p_timers);

	m->m_pkthdr.ph_cookie = em;
	tag->t_state = WG_PKT_STATE_DONE;
	tag->t_ip = peer_route(session->s_peer)->r_ip;
	wg_session_put(session);
	return;
drop:
	m_freem(em);
	counters_inc(tag->t_sc->sc_if.if_counters, ifc_oerrors);
	DPRINTF(tag->t_sc, "failed transport tx: %s\n", wg_error_str[err]);
	tag->t_state = WG_PKT_STATE_DEAD;
}

void
wg_decrypt_hs(struct mbuf *m)
{
	enum wg_error err = WG_OK;

	struct wg_tag *tag;
	struct wg_session *session;

 	tag = wg_mbuf_get_tag(m);
	m_defrag(m, M_WAIT);

	if (tag->t_state != WG_PKT_STATE_NEW)
		panic("not a new state packet: %d", tag->t_state);

	switch (tag->t_type) {
	case WG_PKT_INITIATION:
		if (wg_mbuf_ratelimit(tag->t_sc, m))
			drop_pkt_err(WG_RATELIMIT);

		if ((err = wg_device_rx_initiation(&tag->t_sc->sc_dev, 
		    mtod(m, struct wg_msg_initiation *), &session)) != WG_OK)
			drop_pkt_err(err);

		break;
	case WG_PKT_RESPONSE:
		if (wg_mbuf_ratelimit(tag->t_sc, m))
			drop_pkt_err(WG_RATELIMIT);

		if ((err = wg_device_rx_response(&tag->t_sc->sc_dev, 
		    mtod(m, struct wg_msg_response *), &session)) != WG_OK)
			drop_pkt_err(err);

		wg_timer_reinit_unflag(&session->s_peer->p_timers);
		break;
	case WG_PKT_COOKIE:
		if ((err = wg_device_rx_cookie(&tag->t_sc->sc_dev, 
		    mtod(m, struct wg_msg_cookie *), &session)) != WG_OK)
			drop_pkt_err(err);
		break;
	default:
		panic("not a handshake packet: %d", tag->t_type);
	}

	DPRINTF(tag->t_sc, "rx %s for %x, id %x\n", wg_pkt_str[tag->t_type],
	    session->s_peer->p_id, session->s_local_id);

	tag->t_state = WG_PKT_STATE_DONE;
	peer_route(session->s_peer)->r_ip = tag->t_ip;
	wg_session_put(session);
	return;
drop:
	counters_inc(tag->t_sc->sc_if.if_counters, ifc_ierrors);
	DPRINTF(tag->t_sc, "%s rx failed: %s\n", wg_pkt_str[tag->t_type], wg_error_str[err]);
	tag->t_state = WG_PKT_STATE_DEAD;
}

void
wg_decrypt(struct mbuf *m)
{
	enum wg_error err;
	struct wg_session *session;
	struct wg_tag *tag = wg_mbuf_get_tag(m);
	struct wg_msg_transport *msg = mtod(m, struct wg_msg_transport *);

	m_defrag(m, M_WAIT);

	KASSERT(tag->t_type == WG_PKT_TRANSPORT);
	KASSERT(tag->t_state == WG_PKT_STATE_NEW);

	if ((err = wg_device_rx_transport(&tag->t_sc->sc_dev, msg,
	    m->m_pkthdr.len, &session)) != WG_OK)
		drop_pkt_err(err);

	m_adj(m, sizeof(struct wg_msg_transport));

	if (m->m_pkthdr.len == WG_ENCRYPTED_SIZE(0)) {
		tag->t_state = WG_PKT_STATE_DEAD;
		DPRINTF(tag->t_sc, "recv keepalive %x\n", session->s_local_id);
		goto keepalive;
	}

	if (mtod(m, struct ip *)->ip_v == IPVERSION)
		m->m_pkthdr.ph_family = AF_INET;
#ifdef INET6
	else if ((mtod(m, struct ip6_hdr *)->ip6_vfc & IPV6_VERSION_MASK) ==
	    IPV6_VERSION)
		m->m_pkthdr.ph_family = AF_INET6;
#endif
	else
		drop_pkt_err(WG_INVALID_PKT);

	/* Lookup outgoing route for incoming packet ip src, for crypto key
	 * routing. 'false' indicates src address */
	if (wg_softc_route_lookup(tag->t_sc, m, false) != session->s_peer->p_arg)
		drop_pkt_err(WG_ALLOWED_IP_FAIL);

	counters_pkt(tag->t_sc->sc_if.if_counters, ifc_ipackets, ifc_ibytes,
	    m->m_pkthdr.len);
	wg_timer_keepalive_flag(&session->s_peer->p_timers);

	m->m_pkthdr.ph_ifidx = tag->t_sc->sc_if.if_index;
	m->m_pkthdr.ph_rtableid = tag->t_sc->sc_if.if_rdomain;
	m->m_flags &= ~(M_MCAST | M_BCAST);
	pf_pkt_addr_changed(m);

	tag->t_state = WG_PKT_STATE_DONE;

keepalive:
	wg_timer_persistent_keepalive_tick(&session->s_peer->p_timers);
	wg_timer_broken_unflag(&session->s_peer->p_timers);

	peer_route(session->s_peer)->r_ip = tag->t_ip;
	wg_session_put(session);
	return;
drop:
	counters_inc(tag->t_sc->sc_if.if_counters, ifc_ierrors);
	DPRINTF(tag->t_sc, "failed transport rx: %s\n", wg_error_str[err]);
	tag->t_state = WG_PKT_STATE_DEAD;
}

/* The following functions are the stack facing network functions. They will
 * handle packets in and out for sending and receiving.
 */
void
wg_start(struct ifnet *ifp)
{
	struct mbuf *m;
	struct wg_softc *sc = ifp->if_softc;

	while (!mpq_full(&sc->sc_tx_queue) && (m = ifq_dequeue(&ifp->if_snd)) != NULL)
		mpq_enqueue(&sc->sc_tx_queue, m);
	task_add(sc->sc_taskq, &sc->sc_tx_task);
}

int
wg_output(struct ifnet *ifp, struct mbuf *m, struct sockaddr *sa,
    struct rtentry *rt)
{
	int error;
	struct wg_tag *tag;
	struct wg_route *route;
	struct wg_session *session;
	struct wg_softc *sc = ifp->if_softc;

	if (!AF_VAL(sa->sa_family, 1, 1)) {
		counters_inc(sc->sc_if.if_counters, ifc_noproto);
		m_freem(m);
		return EAFNOSUPPORT;
	}

	if ((tag = wg_mbuf_get_tag(m)) == NULL) {
		counters_inc(sc->sc_if.if_counters, ifc_oerrors);
		m_freem(m);
		return ENOBUFS;
	}

	m->m_pkthdr.ph_family = sa->sa_family;

	route = wg_softc_route_lookup(sc, m, true);

	if (route == NULL || route->r_ip.sa.sa_family == AF_UNSPEC) {
		if (m->m_pkthdr.ph_family == AF_INET)
			icmp_error(m, ICMP_UNREACH, ICMP_UNREACH_HOST, 0, 0);
#ifdef INET6
		else if (m->m_pkthdr.ph_family == AF_INET6)
			icmp6_error(m, ICMP6_DST_UNREACH,
			    ICMP6_DST_UNREACH_ADDR, 0);
#endif
		else
			m_freem(m);
		counters_inc(sc->sc_if.if_counters, ifc_oerrors);
		return ENETUNREACH;
	}

#if NBPFILTER > 0
	if (sc->sc_if.if_bpf)
		bpf_mtap_af(sc->sc_if.if_bpf, m->m_pkthdr.ph_family, m,
		    BPF_DIRECTION_IN);
#endif

	if ((session = wg_peer_ks_session(route->r_peer)) == NULL) {
		if (mq_push(&route->r_outgoing, m) != 0)
			counters_inc(sc->sc_if.if_counters, ifc_oqdrops);
		wg_route_send_initiation(route);
		return 0;
	}

	tag->t_sc = sc;
	tag->t_dst = session->s_local_id;
	tag->t_type = WG_PKT_TRANSPORT;
	tag->t_state = WG_PKT_STATE_NEW;
	wg_session_put(session);

	if ((error = if_enqueue(ifp, m)) != 0) {
		counters_inc(sc->sc_if.if_counters, ifc_oqdrops);
		if_start(ifp);
		return error;
	}

	return 0;
}

struct mbuf *
wg_input(void *_sc, struct mbuf *m, struct sockaddr *sa, int hlen)
{
	struct wg_tag *tag;
	struct wg_softc *sc = _sc;

	if (!ISSET(sc->sc_if.if_flags, IFF_RUNNING))
		goto free;

	m_adj(m, hlen);

	if (m_defrag(m, M_NOWAIT))
		goto leave;

	if ((tag = wg_mbuf_get_tag(m)) == NULL)
		goto free;

	memcpy(&tag->t_ip.sa, sa, sa->sa_len);
	tag->t_sc = sc;
	tag->t_type = wg_pkt_type(mtod(m, uint8_t *), m->m_pkthdr.len);
	tag->t_state = WG_PKT_STATE_NEW;

	switch (tag->t_type) {
	case WG_PKT_INITIATION:
	case WG_PKT_RESPONSE:
	case WG_PKT_COOKIE:
		mpq_enqueue(&sc->sc_rx_slow_queue, m);
		task_add(sc->sc_taskq, &sc->sc_rx_slow_task);
		break;
	case WG_PKT_TRANSPORT:
		mpq_enqueue(&sc->sc_rx_queue, m);
		task_add(sc->sc_taskq, &sc->sc_rx_task);
		break;
	default:
		break;
	}


	return NULL;
free:
	m_freem(m);
leave:
	counters_inc(sc->sc_if.if_counters, ifc_ierrors);
	return NULL;
}

void
wg_input_deliver(struct mbuf *m)
{
	void (*fn_input)(struct ifnet *, struct mbuf *);
	struct wg_tag *tag = wg_mbuf_get_tag(m);
	struct wg_softc *sc = tag->t_sc;

	if (tag->t_state == WG_PKT_STATE_NEW)
		panic("unexpected state on: %p\n", m);
	else if (tag->t_state == WG_PKT_STATE_DEAD)
		m_freem(m);
	else if (tag->t_state == WG_PKT_STATE_DONE) {
#if NBPFILTER > 0
		if (sc->sc_if.if_bpf)
			bpf_mtap_af(sc->sc_if.if_bpf, m->m_pkthdr.ph_family,
			    m, BPF_DIRECTION_OUT);
#endif
		fn_input = AF_VAL(m->m_pkthdr.ph_family, ipv4_input, ipv6_input);
		NET_LOCK();
		if (fn_input)
			fn_input(&sc->sc_if, m);
		else
			panic("invalid af");
		NET_UNLOCK();
	} else {
		panic("invalid state: %d\n", tag->t_state);
	}
}

void
wg_output_deliver(struct mbuf *m)
{
	int error;
	struct socket *so;
	struct wg_softc *sc;
	struct mbuf peernam, *em;
	struct wg_tag *tag = wg_mbuf_get_tag(m);

	if (tag->t_state == WG_PKT_STATE_NEW)
		panic("unexpected state on: %p\n", m);
	else if (tag->t_state == WG_PKT_STATE_DEAD)
		m_freem(m);
	else if (tag->t_state == WG_PKT_STATE_DONE) {
		em = m->m_pkthdr.ph_cookie;
		bzero(&peernam, sizeof(struct mbuf));

		peernam.m_type = MT_SONAME;
		peernam.m_data = (caddr_t) &tag->t_ip;
		peernam.m_len = tag->t_ip.sa.sa_len;

		sc = tag->t_sc;

		so = AF_VAL(tag->t_ip.sa.sa_family, sc->sc_so4, sc->sc_so6);
		if (so) {
			int s = solock(so);
			if ((error = so->so_proto->pr_usrreq(so, PRU_SEND, em,
			    &peernam, NULL, NULL)) != 0)
				DPRINTF(sc, "unable to send: %d\n", error);
			sounlock(so, s);
		} else {
			m_freem(em);
		}

		if (em != m)
			m_freem(m);
	} else {
		panic("invalid state: %d\n", tag->t_state);
	}
}

/* The following functions are for interface control */
struct if_clone wg_cloner =
IF_CLONE_INITIALIZER("wg", wg_clone_create, wg_clone_destroy);

void
wgattach(int nwg)
{
	if_clone_attach(&wg_cloner);
	/* entries: 1024, keys: 3, rate: 5sec, threshold: 5 */
	bb_init(&wg_bb, 1024, 3, 5, 5, M_DEVBUF, M_WAITOK);
}

int
wg_clone_create(struct if_clone * ifc, int unit)
{
	struct ifnet	*ifp;
	struct inpcb	*inp;
	struct wg_softc *sc = malloc(sizeof(*sc), M_DEVBUF, M_WAITOK | M_ZERO);

	/* softc */
	socreate(AF_INET, &sc->sc_so4, SOCK_DGRAM, 0);
	sc->sc_ip_rt = art_alloc(0, 32, 0);
	inp = sotoinpcb(sc->sc_so4);
	inp->inp_upcall = wg_input;
	inp->inp_upcall_arg = sc;

#ifdef INET6
	socreate(AF_INET6, &sc->sc_so6, SOCK_DGRAM, 0);
	sc->sc_ip6_rt = art_alloc(0, 128, 0);
	inp = sotoinpcb(sc->sc_so6);
	inp->inp_upcall = wg_input;
	inp->inp_upcall_arg = sc;
#endif

	wg_device_init(&sc->sc_dev, wg_peer_queue);

	sc->sc_taskq = taskq_create("wg", ncpus, IPL_NET, TASKQ_MPSAFE);
	task_set(&sc->sc_tx_task, wg_tx_task_fn, &sc->sc_tx_queue);
	task_set(&sc->sc_tx_slow_task, wg_tx_slow_task_fn, &sc->sc_tx_slow_queue);
	task_set(&sc->sc_rx_task, wg_rx_task_fn, &sc->sc_rx_queue);
	task_set(&sc->sc_rx_slow_task, wg_rx_slow_task_fn, &sc->sc_rx_slow_queue);
	mpq_init(&sc->sc_tx_queue, IPL_NET);
	mpq_init(&sc->sc_tx_slow_queue, IPL_NET);
	mpq_init(&sc->sc_rx_queue, IPL_NET);
	mpq_init(&sc->sc_rx_slow_queue, IPL_NET);

	/* ifnet */

	ifp = &sc->sc_if;
	ifp->if_softc = sc;

	snprintf(ifp->if_xname, sizeof(ifp->if_xname), "wg%d", unit);

	ifp->if_mtu = 1420;
	ifp->if_flags = IFF_DEBUG | IFF_NOARP | IFF_MULTICAST | IFF_BROADCAST;
	ifp->if_xflags = IFXF_CLONED;

	ifp->if_ioctl = wg_ioctl;
	ifp->if_output = wg_output;
	ifp->if_start = wg_start;
	ifp->if_rtrequest = p2p_rtrequest;

	ifp->if_type = IFT_TUNNEL;
	IFQ_SET_MAXLEN(&ifp->if_snd, IFQ_MAXLEN);

	if_attach(ifp);
	if_alloc_sadl(ifp);
	if_counters_alloc(ifp);

#if NBPFILTER > 0
	bpfattach(&ifp->if_bpf, ifp, DLT_LOOP, sizeof(uint32_t));
#endif

	NET_RLOCK();
	wg_bind_port(sc);
	NET_RUNLOCK();

	DPRINTF(sc, "created interface: %s\n", ifp->if_xname);

	return (0);
}

int
wg_clone_destroy(struct ifnet * ifp)
{
	struct wg_softc *sc = ifp->if_softc;

	CLR(sc->sc_if.if_flags, IFF_RUNNING);

	/* TODO cleanup device */

	taskq_destroy(sc->sc_taskq);

	soclose(sc->sc_so4, 0);
#ifdef INET6
	soclose(sc->sc_so6, 0);
#endif

	if_detach(ifp);

#if NBPFILTER > 0
	bpfdetach(ifp);
#endif

	DPRINTF(sc, "destroyed interface: %s\n", ifp->if_xname);

	free(sc->sc_ip_rt, M_RTABLE, sizeof(*sc->sc_ip_rt));
#ifdef INET6
	free(sc->sc_ip6_rt, M_RTABLE, sizeof(*sc->sc_ip6_rt));
#endif
	free(sc, M_DEVBUF, sizeof(*sc));

	return (0);
}

int
wg_bind_port(struct wg_softc *sc)
{
	int error;
	struct inpcb *inp;
	struct mbuf hostnam;
	struct sockaddr_in *sin;
#ifdef INET6
	struct sockaddr_in6 *sin6;
#endif

	m_inithdr(&hostnam);

	sin = mtod(&hostnam, struct sockaddr_in *);

	/* This just seems dirty */
	soassertlocked(sc->sc_so4);
	inp = sotoinpcb(sc->sc_so4);
	inp->inp_lport = 0;
	inp->inp_laddr = (struct in_addr) { 0 };

	/* Bind ports */
	sin->sin_len = sizeof(*sin);
	sin->sin_family = AF_INET;
	sin->sin_port = htons(sc->sc_port);
	sin->sin_addr = (struct in_addr) { 0 };
	hostnam.m_len = sin->sin_len;

	if ((error = sobind(sc->sc_so4, &hostnam, curproc)) != 0)
		return error;

#ifdef INET6
	sin6 = mtod(&hostnam, struct sockaddr_in6 *);

	soassertlocked(sc->sc_so6);
	inp = sotoinpcb(sc->sc_so6);
	inp->inp_lport = 0;
	inp->inp_laddr6 = (struct in6_addr) { .s6_addr = { 0 } };

	bzero(sin6, sizeof(*sin6));
	sin6->sin6_len = sizeof(*sin6);
	sin6->sin6_family = AF_INET6;
	sin6->sin6_port = htons(sc->sc_port);
	sin6->sin6_addr = (struct in6_addr) { .s6_addr = { 0 } };
	hostnam.m_len = sin6->sin6_len;

	if ((error = sobind(sc->sc_so6, &hostnam, curproc)) != 0)
		return error;
#endif

	return 0;
}

int
wg_ioctl_set_peer(struct wg_softc *sc, u_long cmd, struct wg_set_peer *wsp)
{
	int ret = 0;
	struct wg_peer *peer;
	struct wg_route *route;
	struct wg_route_entry *raip, *traip;

	struct wg_pubkey pub;
	struct wg_privkey shared;

	if (suser(curproc))
		return EPERM;

	/* Silently ignore set peer when the key is for ourselves */
	//if (!memcmp(sc->sc_kp.pub, wsp->sp_pubkey, WG_KEY_SIZE))
	//	return 0;
	if (IS_NULL_KEY(wsp->sp_pubkey) || IS_MASKED_KEY(wsp->sp_pubkey))
		return EINVAL;

	/* TODO compat with byte key */
	memcpy(pub.k, wsp->sp_pubkey, sizeof(pub.k));
	if ((peer = wg_device_ref_peerkey(&sc->sc_dev, &pub)) == NULL) {
		route = malloc(sizeof(*route), M_DEVBUF, M_WAITOK | M_ZERO);
		peer = wg_device_new_peer(&sc->sc_dev, &pub, route);
		wg_peer_ref(peer);

		route->r_sc = sc;
		route->r_peer = peer;
		mq_init(&route->r_outgoing, IFQ_MAXLEN, IPL_NET);
		SLIST_INIT(&route->r_aip);

		wg_timer_setup(&peer->p_timers, route,
		    (void (*)(void *)) wg_route_send_keepalive,
		    (void (*)(void *)) wg_route_broken,
		    (void (*)(void *)) wg_route_send_initiation,
		    (void (*)(void *)) wg_route_clean);

		DPRINTF(sc, "added peer: %x\n", peer->p_id);
	} else {
		route = peer->p_arg;
	}

	switch (cmd) {
	case SIOCSWGPEERIP:
		route->r_ip = wsp->sp_ip;
		break;
	case SIOCSWGPEERPSK:
		if (IS_NULL_KEY(wsp->sp_psk) || IS_MASKED_KEY(wsp->sp_psk))
			return EINVAL;
		/* TODO compat with byte key */
		memcpy(shared.k, wsp->sp_psk, sizeof(shared.k));
		wg_peer_setshared(peer, &shared);
		break;
	case SIOCSWGPEERPKA:
		wg_timer_persistent_keepalive_set(&peer->p_timers, wsp->sp_pka);
		break;
	case SIOCDWGPEER:
		DPRINTF(sc, "deleted peer: %x\n", peer->p_id);
		SLIST_FOREACH_SAFE(raip, &route->r_aip, r_entry, traip)
			wg_softc_route_delete(sc, &raip->r_cidr);
		wg_peer_put(peer);
		free(peer->p_arg, M_DEVBUF, sizeof(struct wg_route));
		wg_peer_drop(peer);
		break;
	case SIOCSWGPEERAIP:
		ret = wg_softc_route_add(sc, &wsp->sp_aip, route);
		break;
	case SIOCDWGPEERAIP:
		ret = wg_softc_route_delete(sc, &wsp->sp_aip);
		break;
	case SIOCCWGPEERAIP:
		SLIST_FOREACH_SAFE(raip, &route->r_aip, r_entry, traip)
			wg_softc_route_delete(sc, &raip->r_cidr);
		break;
	}

	wg_peer_put(peer);

	return ret;
}

int
wg_ioctl_set_serv(struct wg_softc *sc, u_long cmd, struct wg_set_serv *wss)
{
	struct wg_peer *peer;
	struct wg_keypair kp;
	struct wg_privkey priv;

	if (suser(curproc))
		return EPERM;

	switch (cmd) {
	case SIOCSWGSERVPORT: /* Set WireGuard server port */
		sc->sc_port = wss->ss_port;
		return wg_bind_port(sc);
	case SIOCSWGSERVKEY: /* Set WireGuard server private + public key */
		/* Check that the new key does not belong to a peer */
		memcpy(priv.k, wss->ss_privkey, sizeof(priv.k));
		wg_keypair_from_key(&kp, &priv);
		peer = wg_device_ref_peerkey(&sc->sc_dev, &kp.pub);
		if (peer != NULL){
			wg_peer_put(peer);
			return ENOTSUP;
		}
		if (IS_NULL_KEY(wss->ss_privkey) ||
		    IS_MASKED_KEY(wss->ss_privkey))
			return EINVAL;

		wg_device_setkey(&sc->sc_dev, &priv);
		break;
	case SIOCCWGPEERS: /* Clear all WireGuard peers */
		break;
	}
	return 0;
}

void
wg_ioctl_get_serv(struct wg_softc *sc, struct wg_get_serv *wgs)
{
	size_t num = 0;
	/*struct wg_peer *peer; */
	struct wg_keypair *kp;
	//uint8_t key[WG_KEY_SIZE];

	wgs->gs_port = sc->sc_port;
	kp = &sc->sc_dev.d_keypair;
	memcpy(wgs->gs_pubkey, kp->pub.k, sizeof(wgs->gs_pubkey));

	/* We only want to pass the private key to root */
	if (!suser(curproc) || IS_NULL_KEY(kp->priv.k))
		memcpy(wgs->gs_privkey, kp->priv.k, sizeof(wgs->gs_privkey));
	else
		memset(wgs->gs_privkey, 0xff, sizeof(wgs->gs_privkey));

	struct map_item *item;
	struct wg_peer *peer;

	/* For the time being, no lock as we hold kernel lock in ioctl */
	FM_FOREACH_FILLED(item, &sc->sc_dev.d_peers) {
		if (num < wgs->gs_num_peers) {
			peer = item->value;
			copyout(peer->p_remote.k, wgs->gs_peers[num], WG_KEY_SIZE);
		}
		num++;
	}
	wgs->gs_num_peers = num;
}

int
wg_ioctl_get_peer(struct wg_softc *sc, struct wg_get_peer *wgp)
{
	size_t num = 0;
	struct wg_pubkey pub;
	struct wg_privkey shared;
	struct wg_peer *peer;
	struct wg_route *route;

	memcpy(pub.k, wgp->gp_pubkey, sizeof(pub.k));

	if ((peer = wg_device_ref_peerkey(&sc->sc_dev, &pub)) == NULL)
		return ENOENT;

	route = peer->p_arg;

	/* We only want to pass the preshared key to root */
	wg_peer_getshared(peer, &shared);
	if (!suser(curproc) || IS_NULL_KEY(shared.k))
		memcpy(wgp->gp_psk, shared.k, sizeof(wgp->gp_psk));
	else
		memset(wgp->gp_psk, 0xff, sizeof(wgp->gp_psk));

	wgp->gp_last_handshake = wg_peer_last_handshake(peer);
	wgp->gp_pka = wg_timer_persistent_keepalive_get(&peer->p_timers);

	wgp->gp_tx_bytes = peer->p_tx_bytes;
	wgp->gp_rx_bytes = peer->p_rx_bytes;
	wgp->gp_ip = route->r_ip;

	struct wg_route_entry *r;
	SLIST_FOREACH(r, &route->r_aip, r_entry) {
		if (num < wgp->gp_num_aip)
			copyout(&r->r_cidr, &wgp->gp_aip[num], sizeof(*wgp->gp_aip));
		num++;
	}
	wgp->gp_num_aip = num;
	wg_peer_put(peer);
	return 0;
}

int
wg_ioctl(struct ifnet * ifp, u_long cmd, caddr_t data)
{
	int ret = 0;
	struct wg_softc *sc = ifp->if_softc;
	struct ifreq *ifr = (struct ifreq *) data;

	switch (cmd) {
	/* WireGuard IOCTLs */
	case SIOCSWGSERVPORT: /* Set WireGuard server port */
	case SIOCSWGSERVKEY: /* Set WireGuard server private + public key */
	case SIOCCWGPEERS: /* Clear all WireGuard peers */
		ret = wg_ioctl_set_serv(sc, cmd, (struct wg_set_serv *) data);
		break;
	case SIOCSWGPEERIP: /* Set WireGuard peer endpoint IP */
	case SIOCSWGPEERPSK: /* Set WireGuard peer preshared key */
	case SIOCSWGPEERAIP: /* Add WireGuard peer allowedip */
	case SIOCSWGPEERPKA: /* Set WireGuard peer persistent keepalive */
	case SIOCDWGPEER: /* Delete WireGuard peer */
	case SIOCDWGPEERAIP: /* Delete WireGuard peer allowedip */
	case SIOCCWGPEERAIP: /* Clear WireGuard peer */
		ret = wg_ioctl_set_peer(sc, cmd, (struct wg_set_peer *) data);
		break;
	case SIOCGWGSERV: /* Get WireGuard server configuration */
		wg_ioctl_get_serv(sc, (struct wg_get_serv *) data);
		break;
	case SIOCGWGPEER: /* Get WireGuard peer status */
		wg_ioctl_get_peer(sc, (struct wg_get_peer *) data);
		break;

	/* Interface IOCTLs */
	case SIOCSIFADDR:
		ifp->if_flags |= IFF_UP;
		/* FALLTHROUGH */
	case SIOCSIFFLAGS:
		if (ISSET(ifp->if_flags, IFF_UP)) {
			if (!ISSET(ifp->if_flags, IFF_RUNNING)) {
				SET(sc->sc_if.if_flags, IFF_RUNNING);
			}
		} else {
			if (ISSET(ifp->if_flags, IFF_RUNNING)) {
				CLR(sc->sc_if.if_flags, IFF_RUNNING);
			}
		}
		break;
	case SIOCSIFMTU:
		/* Arbitrary limits */
		if (ifr->ifr_mtu <= 0 || ifr->ifr_mtu > 9000)
			ret = EINVAL;
		else
			ifp->if_mtu = ifr->ifr_mtu;
		break;
	case SIOCSLIFPHYRTABLE:
		if (ifr->ifr_rdomainid < 0 ||
		    ifr->ifr_rdomainid > RT_TABLEID_MAX ||
		    !rtable_exists(ifr->ifr_rdomainid)) {
			ret = EINVAL;
		} else {
			sotoinpcb(sc->sc_so4)->inp_rtableid =
			    ifr->ifr_rdomainid;
#ifdef INET6
			sotoinpcb(sc->sc_so6)->inp_rtableid =
			    ifr->ifr_rdomainid;
#endif
			ret = wg_bind_port(sc);
		}
		break;
	case SIOCGLIFPHYRTABLE:
		ifr->ifr_rdomainid = sotoinpcb(sc->sc_so4)->inp_rtableid;
		break;

	case SIOCADDMULTI:
	case SIOCDELMULTI:
		break;

	default:
		ret = ENOTTY;
	}

	return ret;
}