Replace wg_tag with wg_packet

I'll be the first to admit (but not the first to complain) about the
wg_tag situation. It made it very difficult to manage mbufs (that may be
reallocated with functions such as m_pullup). It was also not clear
where allocation was occuring. This also gets rid of the ring buffers in
wg_softc, which added no performance in this situation. They also used
memory unnecessarily and increased the complexity.

I also used this opportunity to get rid of the confusing t_mbuf/t_done
situation and revert to a more understandable UNCRYPTED/CRYPTED/DEAD
packet state. I don't believe there were any issues with the old style,
but to improve readability is always a welcome addition.

With these changes we can start encrypting packets in place (rather than
copying to a new mbuf), which should increase performance. This also
simplifies length calculations by using m_* functions and reading the
pkthdr length.

1 files changed, 292 insertions, 291 deletions

diff --git a/sys/net/if_wg.c b/sys/net/if_wg.c
index 7afb59ba464..e34f5409d12 100644
--- a/sys/net/if_wg.c
+++ b/sys/net/if_wg.c
@@ -135,14 +135,6 @@ struct wg_endpoint {
 	} e_local;
 };
 
-struct wg_tag {
-	struct wg_endpoint	 t_endpoint;
-	struct wg_peer		*t_peer;
-	struct mbuf		*t_mbuf;
-	int			 t_done;
-	int			 t_mtu;
-};
-
 struct wg_index {
 	LIST_ENTRY(wg_index)	 i_entry;
 	SLIST_ENTRY(wg_index)	 i_unused_entry;
@@ -176,16 +168,29 @@ struct wg_aip {
 	struct wg_aip_io	 a_data;
 };
 
-struct wg_queue {
-	struct mutex		 q_mtx;
-	struct mbuf_list	 q_list;
+struct wg_tag {
+	struct wg_peer		*t_peer;
+	int			 t_mtu;
 };
 
-struct wg_ring {
-	struct mutex	 r_mtx;
-	uint32_t	 r_head;
-	uint32_t	 r_tail;
-	struct mbuf	*r_buf[MAX_QUEUED_PKT];
+struct wg_packet {
+	SIMPLEQ_ENTRY(wg_packet) p_serial;
+	SIMPLEQ_ENTRY(wg_packet) p_parallel;
+	struct wg_endpoint	 p_endpoint;
+	struct wg_peer		*p_peer;
+	struct mbuf		*p_mbuf;
+	int			 p_mtu;
+	enum wg_ring_state {
+		WG_PACKET_UNCRYPTED,
+		WG_PACKET_CRYPTED,
+		WG_PACKET_DEAD,
+	}			 p_state;
+};
+
+struct wg_queue {
+	struct mutex		 q_mtx;
+	SIMPLEQ_HEAD(,wg_packet) q_queue;
+	size_t			 q_len;
 };
 
 struct wg_peer {
@@ -212,8 +217,8 @@ struct wg_peer {
 	struct task		 p_deliver_in;
 
 	struct mbuf_queue	 p_stage_queue;
-	struct wg_queue		 p_encap_queue;
-	struct wg_queue		 p_decap_queue;
+	struct wg_queue		 p_encap_serial;
+	struct wg_queue		 p_decap_serial;
 
 	SLIST_HEAD(,wg_index)	 p_unused_index;
 	struct wg_index		 p_index[3];
@@ -261,8 +266,8 @@ struct wg_softc {
 
 	struct task		 sc_encap;
 	struct task		 sc_decap;
-	struct wg_ring		 sc_encap_ring;
-	struct wg_ring		 sc_decap_ring;
+	struct wg_queue		 sc_encap_parallel;
+	struct wg_queue		 sc_decap_parallel;
 };
 
 struct wg_peer *
@@ -271,7 +276,7 @@ struct wg_peer *
 struct wg_peer *
 	wg_peer_lookup(struct wg_softc *, const uint8_t[WG_KEY_SIZE]);
 void	wg_peer_destroy(struct wg_peer *);
-void	wg_peer_set_endpoint_from_tag(struct wg_peer *, struct wg_tag *);
+void	wg_peer_set_endpoint(struct wg_peer *, struct wg_endpoint *);
 void	wg_peer_set_sockaddr(struct wg_peer *, struct sockaddr *);
 int	wg_peer_get_sockaddr(struct wg_peer *, struct sockaddr *);
 void	wg_peer_clear_src(struct wg_peer *);
@@ -330,23 +335,24 @@ void	wg_send_cookie(struct wg_softc *, struct cookie_macs *, uint32_t,
 	    struct wg_endpoint *e);
 void	wg_send_keepalive(void *);
 void	wg_peer_clear_secrets(void *);
-void	wg_handshake(struct wg_softc *, struct mbuf *);
+void	wg_handshake(struct wg_softc *, struct wg_packet *);
 void	wg_handshake_worker(void *);
 
-void	wg_encap(struct wg_softc *, struct mbuf *);
-void	wg_decap(struct wg_softc *, struct mbuf *);
+void	wg_encap(struct wg_softc *, struct wg_packet *);
+void	wg_decap(struct wg_softc *, struct wg_packet *);
 void	wg_encap_worker(void *);
 void	wg_decap_worker(void *);
 void	wg_deliver_out(void *);
 void	wg_deliver_in(void *);
 
-int	wg_queue_in(struct wg_softc *, struct wg_peer *, struct mbuf *);
+void	wg_queue_init(struct wg_queue *);
+int	wg_queue_both(struct wg_queue *, struct wg_queue *, struct wg_packet *);
+void	wg_queue_in(struct wg_softc *, struct wg_peer *, struct wg_packet *);
 void	wg_queue_out(struct wg_softc *, struct wg_peer *);
-struct mbuf *
-	wg_ring_dequeue(struct wg_ring *);
-struct mbuf *
-	wg_queue_dequeue(struct wg_queue *, struct wg_tag **);
-size_t	wg_queue_len(struct wg_queue *);
+struct wg_packet *
+	wg_queue_serial_dequeue(struct wg_queue *);
+struct wg_packet *
+	wg_queue_parallel_dequeue(struct wg_queue *);
 
 struct noise_remote *
 	wg_remote_get(void *, uint8_t[NOISE_PUBLIC_KEY_LEN]);
@@ -359,6 +365,7 @@ void	wg_index_drop(void *, uint32_t);
 struct mbuf *
 	wg_input(void *, struct mbuf *, struct ip *, struct ip6_hdr *, void *,
 	    int);
+void	wg_qstart(struct ifqueue *);
 int	wg_output(struct ifnet *, struct mbuf *, struct sockaddr *,
 	    struct rtentry *);
 int	wg_ioctl_set(struct wg_softc *, struct wg_data_io *);
@@ -375,6 +382,7 @@ uint64_t	peer_counter = 0;
 uint64_t	keypair_counter = 0;
 struct pool	wg_aip_pool;
 struct pool	wg_peer_pool;
+struct pool	wg_packet_pool;
 struct pool	wg_ratelimit_pool;
 struct timeval	underload_interval = { UNDERLOAD_TIMEOUT, 0 };
 
@@ -422,10 +430,8 @@ wg_peer_create(struct wg_softc *sc, uint8_t public[WG_KEY_SIZE],
 	task_set(&peer->p_deliver_in, wg_deliver_in, peer);
 
 	mq_init(&peer->p_stage_queue, MAX_STAGED_PKT, IPL_NET);
-	mtx_init(&peer->p_encap_queue.q_mtx, IPL_NET);
-	ml_init(&peer->p_encap_queue.q_list);
-	mtx_init(&peer->p_decap_queue.q_mtx, IPL_NET);
-	ml_init(&peer->p_decap_queue.q_list);
+	wg_queue_init(&peer->p_encap_serial);
+	wg_queue_init(&peer->p_decap_serial);
 
 	SLIST_INIT(&peer->p_unused_index);
 	SLIST_INSERT_HEAD(&peer->p_unused_index, &peer->p_index[0],
@@ -525,14 +531,13 @@ wg_peer_destroy(struct wg_peer *peer)
 }
 
 void
-wg_peer_set_endpoint_from_tag(struct wg_peer *peer, struct wg_tag *t)
+wg_peer_set_endpoint(struct wg_peer *peer, struct wg_endpoint *e)
 {
-	if (memcmp(&t->t_endpoint, &peer->p_endpoint,
-	    sizeof(t->t_endpoint)) == 0)
+	if (memcmp(e, &peer->p_endpoint, sizeof(*e)) == 0)
 		return;
 
 	mtx_enter(&peer->p_endpoint_mtx);
-	peer->p_endpoint = t->t_endpoint;
+	peer->p_endpoint = *e;
 	mtx_leave(&peer->p_endpoint_mtx);
 }
 
@@ -813,6 +818,7 @@ wg_send(struct wg_softc *sc, struct wg_endpoint *e, struct mbuf *m)
 			    IPPROTO_IPV6);
 #endif
 	} else {
+		m_freem(m);
 		return EAFNOSUPPORT;
 	}
 
@@ -873,7 +879,7 @@ retry:
 struct wg_tag *
 wg_tag_get(struct mbuf *m)
 {
-	struct m_tag	*mtag;
+	struct m_tag    *mtag;
 
 	if ((mtag = m_tag_find(m, PACKET_TAG_WIREGUARD, NULL)) == NULL) {
 		mtag = m_tag_get(PACKET_TAG_WIREGUARD, sizeof(struct wg_tag),
@@ -1277,10 +1283,10 @@ wg_send_cookie(struct wg_softc *sc, struct cookie_macs *cm, uint32_t idx,
 void
 wg_send_keepalive(void *_peer)
 {
-	struct wg_peer	*peer = _peer;
-	struct wg_softc	*sc = peer->p_sc;
-	struct wg_tag	*t;
-	struct mbuf	*m;
+	struct wg_peer		*peer = _peer;
+	struct wg_softc		*sc = peer->p_sc;
+	struct wg_packet	*pkt;
+	struct mbuf		*m;
 
 	if (!mq_empty(&peer->p_stage_queue))
 		goto send;
@@ -1288,27 +1294,23 @@ wg_send_keepalive(void *_peer)
 	if ((m = m_gethdr(M_NOWAIT, MT_DATA)) == NULL)
 		return;
 
-	if ((t = wg_tag_get(m)) == NULL) {
+	if ((pkt = pool_get(&wg_packet_pool, PR_NOWAIT)) == NULL) {
+		counters_inc(sc->sc_if.if_counters, ifc_iqdrops);
 		m_freem(m);
 		return;
 	}
 
+	pkt->p_mbuf = m;
+	pkt->p_peer = peer;
+	pkt->p_mtu = 0;
+
+	m->m_pkthdr.ph_cookie = pkt;
 	m->m_len = 0;
 	m_calchdrlen(m);
 
-	t->t_peer = peer;
-	t->t_mbuf = NULL;
-	t->t_done = 0;
-	t->t_mtu = 0; /* MTU == 0 OK for keepalive */
-
 	mq_push(&peer->p_stage_queue, m);
 send:
-	if (noise_remote_ready(&peer->p_remote) == 0) {
-		wg_queue_out(sc, peer);
-		task_add(wg_crypt_taskq, &sc->sc_encap);
-	} else {
-		wg_timers_event_want_initiation(&peer->p_timers);
-	}
+	wg_queue_out(peer->p_sc, peer);
 }
 
 void
@@ -1319,13 +1321,14 @@ wg_peer_clear_secrets(void *_peer)
 }
 
 void
-wg_handshake(struct wg_softc *sc, struct mbuf *m)
+wg_handshake(struct wg_softc *sc, struct wg_packet *pkt)
 {
-	struct wg_tag			*t;
 	struct wg_pkt_initiation	*init;
 	struct wg_pkt_response		*resp;
 	struct wg_pkt_cookie		*cook;
+	struct wg_endpoint		*e;
 	struct wg_peer			*peer;
+	struct mbuf			*m;
 	struct noise_remote		*remote;
 	int				 res, underload = 0;
 	static struct timeval		 wg_last_underload; /* microuptime */
@@ -1340,7 +1343,8 @@ wg_handshake(struct wg_softc *sc, struct mbuf *m)
 			bzero(&wg_last_underload, sizeof(wg_last_underload));
 	}
 
-	t = wg_tag_get(m);
+	m = pkt->p_mbuf;
+	e = &pkt->p_endpoint;
 
 	switch (*mtod(m, uint32_t *)) {
 	case WG_PKT_INITIATION:
@@ -1348,7 +1352,7 @@ wg_handshake(struct wg_softc *sc, struct mbuf *m)
 
 		res = cookie_checker_validate_macs(&sc->sc_cookie, &init->m,
 				init, sizeof(*init) - sizeof(init->m),
-				underload, &t->t_endpoint.e_remote.r_sa);
+				underload, &e->e_remote.r_sa);
 
 		if (res == EINVAL) {
 			DPRINTF(sc, "Invalid initiation MAC\n");
@@ -1357,8 +1361,7 @@ wg_handshake(struct wg_softc *sc, struct mbuf *m)
 			DPRINTF(sc, "Handshake ratelimited\n");
 			goto error;
 		} else if (res == EAGAIN) {
-			wg_send_cookie(sc, &init->m, init->s_idx,
-			    &t->t_endpoint);
+			wg_send_cookie(sc, &init->m, init->s_idx, e);
 			goto error;
 		} else if (res != 0) {
 			panic("unexpected response: %d\n", res);
@@ -1375,7 +1378,7 @@ wg_handshake(struct wg_softc *sc, struct mbuf *m)
 		DPRINTF(sc, "Receiving handshake initiation from peer %llu\n",
 		    peer->p_id);
 
-		wg_peer_set_endpoint_from_tag(peer, t);
+		wg_peer_set_endpoint(peer, e);
 		wg_send_response(peer);
 		break;
 	case WG_PKT_RESPONSE:
@@ -1383,7 +1386,7 @@ wg_handshake(struct wg_softc *sc, struct mbuf *m)
 
 		res = cookie_checker_validate_macs(&sc->sc_cookie, &resp->m,
 				resp, sizeof(*resp) - sizeof(resp->m),
-				underload, &t->t_endpoint.e_remote.r_sa);
+				underload, &e->e_remote.r_sa);
 
 		if (res == EINVAL) {
 			DPRINTF(sc, "Invalid response MAC\n");
@@ -1392,8 +1395,7 @@ wg_handshake(struct wg_softc *sc, struct mbuf *m)
 			DPRINTF(sc, "Handshake ratelimited\n");
 			goto error;
 		} else if (res == EAGAIN) {
-			wg_send_cookie(sc, &resp->m, resp->s_idx,
-			    &t->t_endpoint);
+			wg_send_cookie(sc, &resp->m, resp->s_idx, e);
 			goto error;
 		} else if (res != 0) {
 			panic("unexpected response: %d\n", res);
@@ -1415,7 +1417,7 @@ wg_handshake(struct wg_softc *sc, struct mbuf *m)
 		DPRINTF(sc, "Receiving handshake response from peer %llu\n",
 				peer->p_id);
 
-		wg_peer_set_endpoint_from_tag(peer, t);
+		wg_peer_set_endpoint(peer, e);
 		if (noise_remote_begin_session(&peer->p_remote) == 0) {
 			wg_timers_event_session_derived(&peer->p_timers);
 			wg_timers_event_handshake_complete(&peer->p_timers);
@@ -1448,6 +1450,7 @@ wg_handshake(struct wg_softc *sc, struct mbuf *m)
 	wg_peer_counters_add(peer, 0, m->m_pkthdr.len);
 error:
 	m_freem(m);
+	pool_put(&wg_packet_pool, pkt);
 }
 
 void
@@ -1456,7 +1459,7 @@ wg_handshake_worker(void *_sc)
 	struct mbuf *m;
 	struct wg_softc *sc = _sc;
 	while ((m = mq_dequeue(&sc->sc_handshake_queue)) != NULL)
-		wg_handshake(sc, m);
+		wg_handshake(sc, m->m_pkthdr.ph_cookie);
 }
 
 /*
@@ -1470,33 +1473,24 @@ wg_handshake_worker(void *_sc)
  *    threads.
  *  - The serial queue ensures that packets are not reordered and are
  *    delievered in sequence.
- * The wg_tag attached to the packet contains two flags to help the two queues
- * interact.
- *  - t_done: The parallel queue has finished with the packet, now the serial
- *            queue can do it's work.
- *  - t_mbuf: Used to store the *crypted packet. in the case of encryption,
- *            this is a newly allocated packet, and in the case of decryption,
- *            it is a pointer to the same packet, that has been decrypted and
- *            truncated. If t_mbuf is NULL, then *cryption failed and this
- *            packet should not be passed.
+ *
  * wg_{en,de}cap work on the parallel queue, while wg_deliver_{in,out} work
  * on the serial queue.
  */
 void
-wg_encap(struct wg_softc *sc, struct mbuf *m)
+wg_encap(struct wg_softc *sc, struct wg_packet *pkt)
 {
-	int res = 0;
 	struct wg_pkt_data	*data;
 	struct wg_peer		*peer;
-	struct wg_tag		*t;
-	struct mbuf		*mc;
+	struct mbuf		*m, *mc;
 	size_t			 padding_len, plaintext_len, out_len;
 	uint64_t		 nonce;
+	int			 res;
 
-	t = wg_tag_get(m);
-	peer = t->t_peer;
+	peer = pkt->p_peer;
+	m = pkt->p_mbuf;
 
-	plaintext_len = min(WG_PKT_WITH_PADDING(m->m_pkthdr.len), t->t_mtu);
+	plaintext_len = min(WG_PKT_WITH_PADDING(m->m_pkthdr.len), pkt->p_mtu);
 	padding_len = plaintext_len - m->m_pkthdr.len;
 	out_len = sizeof(struct wg_pkt_data) + plaintext_len + NOISE_AUTHTAG_LEN;
 
@@ -1557,29 +1551,35 @@ wg_encap(struct wg_softc *sc, struct mbuf *m)
 	 */
 	wg_peer_counters_add(peer, mc->m_pkthdr.len, 0);
 
-	t->t_mbuf = mc;
+	/* TODO this is temporary, and will be replaced with proper mbuf handling */
+	m_freem(m);
+	pkt->p_mbuf = mc;
+
+	pkt->p_state = WG_PACKET_CRYPTED;
+	task_add(net_tq(sc->sc_if.if_index), &peer->p_deliver_out);
+	return;
 error:
-	t->t_done = 1;
+	pkt->p_state = WG_PACKET_DEAD;
 	task_add(net_tq(sc->sc_if.if_index), &peer->p_deliver_out);
 }
 
 void
-wg_decap(struct wg_softc *sc, struct mbuf *m)
+wg_decap(struct wg_softc *sc, struct wg_packet *pkt)
 {
-	int			 res, len;
 	struct ip		*ip;
 	struct ip6_hdr		*ip6;
 	struct wg_pkt_data	*data;
 	struct wg_peer		*peer, *allowed_peer;
-	struct wg_tag		*t;
+	struct mbuf		*m;
 	size_t			 payload_len;
 	uint64_t		 nonce;
+	int			 res, len;
 
-	t = wg_tag_get(m);
-	peer = t->t_peer;
+	peer = pkt->p_peer;
+	m = pkt->p_mbuf;
 
 	/*
-	 * Likewise to wg_encap, we pass a buf and buf length to 
+	 * Likewise to wg_encap, we pass a buf and buf length to
 	 * noise_remote_decrypt. Again, possible to teach it about mbufs
 	 * but need to get over the p_decap_queue situation first. However,
 	 * we do not need to allocate a new mbuf as the decrypted packet is
@@ -1603,7 +1603,7 @@ wg_decap(struct wg_softc *sc, struct mbuf *m)
 		panic("unexpected response: %d\n", res);
 	}
 
-	wg_peer_set_endpoint_from_tag(peer, t);
+	wg_peer_set_endpoint(peer, &pkt->p_endpoint);
 
 	wg_peer_counters_add(peer, 0, m->m_pkthdr.len);
 
@@ -1676,28 +1676,30 @@ wg_decap(struct wg_softc *sc, struct mbuf *m)
 #endif /* NPF > 0 */
 
 done:
-	t->t_mbuf = m;
+	pkt->p_state = WG_PACKET_CRYPTED;
+	task_add(net_tq(sc->sc_if.if_index), &peer->p_deliver_in);
+	return;
 error:
-	t->t_done = 1;
+	pkt->p_state = WG_PACKET_DEAD;
 	task_add(net_tq(sc->sc_if.if_index), &peer->p_deliver_in);
 }
 
 void
 wg_encap_worker(void *_sc)
 {
-	struct mbuf *m;
+	struct wg_packet *pkt;
 	struct wg_softc *sc = _sc;
-	while ((m = wg_ring_dequeue(&sc->sc_encap_ring)) != NULL)
-		wg_encap(sc, m);
+	while ((pkt = wg_queue_parallel_dequeue(&sc->sc_encap_parallel)) != NULL)
+		wg_encap(sc, pkt);
 }
 
 void
 wg_decap_worker(void *_sc)
 {
-	struct mbuf *m;
+	struct wg_packet *pkt;
 	struct wg_softc *sc = _sc;
-	while ((m = wg_ring_dequeue(&sc->sc_decap_ring)) != NULL)
-		wg_decap(sc, m);
+	while ((pkt = wg_queue_parallel_dequeue(&sc->sc_decap_parallel)) != NULL)
+		wg_decap(sc, pkt);
 }
 
 void
@@ -1705,116 +1707,122 @@ wg_deliver_out(void *_peer)
 {
 	struct wg_peer		*peer = _peer;
 	struct wg_softc		*sc = peer->p_sc;
+	struct wg_packet	*pkt;
 	struct wg_endpoint	 endpoint;
-	struct wg_tag		*t;
 	struct mbuf		*m;
-	int			 ret;
+	int			 ret, data;
 
 	wg_peer_get_endpoint(peer, &endpoint);
 
-	while ((m = wg_queue_dequeue(&peer->p_encap_queue, &t)) != NULL) {
-		/* t_mbuf will contain the encrypted packet */
-		if (t->t_mbuf == NULL){
-			counters_inc(sc->sc_if.if_counters, ifc_oerrors);
-			m_freem(m);
-			continue;
-		}
+	while ((pkt = wg_queue_serial_dequeue(&peer->p_encap_serial)) != NULL) {
+		m = pkt->p_mbuf;
+		if (pkt->p_state == WG_PACKET_CRYPTED) {
 
-		ret = wg_send(sc, &endpoint, t->t_mbuf);
+			data = m->m_pkthdr.len > (sizeof(struct wg_pkt_data) + NOISE_AUTHTAG_LEN);
+			ret = wg_send(sc, &endpoint, m);
 
-		if (ret == 0) {
-			wg_timers_event_any_authenticated_packet_traversal(
-			    &peer->p_timers);
-			wg_timers_event_any_authenticated_packet_sent(
-			    &peer->p_timers);
+			if (ret == 0) {
+				wg_timers_event_any_authenticated_packet_traversal(
+				    &peer->p_timers);
+				wg_timers_event_any_authenticated_packet_sent(
+				    &peer->p_timers);
 
-			if (m->m_pkthdr.len != 0)
-				wg_timers_event_data_sent(&peer->p_timers);
-		} else if (ret == EADDRNOTAVAIL) {
-			wg_peer_clear_src(peer);
-			wg_peer_get_endpoint(peer, &endpoint);
+				if (data)
+					wg_timers_event_data_sent(&peer->p_timers);
+			} else if (ret == EADDRNOTAVAIL) {
+				wg_peer_clear_src(peer);
+				wg_peer_get_endpoint(peer, &endpoint);
+			}
+
+		} else {
+			m_freem(m);
 		}
 
-		m_freem(m);
+		pool_put(&wg_packet_pool, pkt);
 	}
 }
 
 void
 wg_deliver_in(void *_peer)
 {
-	struct wg_peer	*peer = _peer;
-	struct wg_softc	*sc = peer->p_sc;
-	struct wg_tag	*t;
-	struct mbuf	*m;
-
-	while ((m = wg_queue_dequeue(&peer->p_decap_queue, &t)) != NULL) {
-		/* t_mbuf will contain the decrypted packet */
-		if (t->t_mbuf == NULL) {
-			counters_inc(sc->sc_if.if_counters, ifc_ierrors);
-			m_freem(m);
-			continue;
-		}
-
-		/* From here on m == t->t_mbuf */
-		KASSERT(m == t->t_mbuf);
+	struct wg_peer		*peer = _peer;
+	struct wg_softc		*sc = peer->p_sc;
+	struct wg_packet	*pkt;
+	struct mbuf		*m;
 
-		wg_timers_event_any_authenticated_packet_received(
-		    &peer->p_timers);
-		wg_timers_event_any_authenticated_packet_traversal(
-		    &peer->p_timers);
+	while ((pkt = wg_queue_serial_dequeue(&peer->p_decap_serial)) != NULL) {
+		m = pkt->p_mbuf;
+		if (pkt->p_state == WG_PACKET_CRYPTED) {
+			wg_timers_event_any_authenticated_packet_received(
+			    &peer->p_timers);
+			wg_timers_event_any_authenticated_packet_traversal(
+			    &peer->p_timers);
 
-		if (m->m_pkthdr.len == 0) {
-			m_freem(m);
-			continue;
-		}
+			if (m->m_pkthdr.len == 0) {
+				m_freem(m);
+				continue;
+			}
 
 #if NBPFILTER > 0
-		if (sc->sc_if.if_bpf != NULL)
-			bpf_mtap_af(sc->sc_if.if_bpf,
-			    m->m_pkthdr.ph_family, m, BPF_DIRECTION_IN);
+			if (sc->sc_if.if_bpf != NULL)
+				bpf_mtap_af(sc->sc_if.if_bpf,
+				    m->m_pkthdr.ph_family, m, BPF_DIRECTION_IN);
 #endif
 
-		NET_LOCK();
-		if (m->m_pkthdr.ph_family == AF_INET)
-			ipv4_input(&sc->sc_if, m);
+			NET_LOCK();
+			if (m->m_pkthdr.ph_family == AF_INET)
+				ipv4_input(&sc->sc_if, m);
 #ifdef INET6
-		else if (m->m_pkthdr.ph_family == AF_INET6)
-			ipv6_input(&sc->sc_if, m);
+			else if (m->m_pkthdr.ph_family == AF_INET6)
+				ipv6_input(&sc->sc_if, m);
 #endif
-		else
-			panic("invalid ph_family");
-		NET_UNLOCK();
+			else
+				panic("invalid ph_family");
+			NET_UNLOCK();
+
+			wg_timers_event_data_received(&peer->p_timers);
+
+		} else {
+			m_freem(m);
+		}
 
-		wg_timers_event_data_received(&peer->p_timers);
+		pool_put(&wg_packet_pool, pkt);
 	}
 }
 
+void
+wg_queue_init(struct wg_queue *queue)
+{
+	mtx_init(&queue->q_mtx, IPL_NET);
+	SIMPLEQ_INIT(&queue->q_queue);
+	queue->q_len = 0;
+}
+
 int
-wg_queue_in(struct wg_softc *sc, struct wg_peer *peer, struct mbuf *m)
+wg_queue_both(struct wg_queue *parallel, struct wg_queue *serial, struct wg_packet *pkt)
 {
-	struct wg_ring		*parallel = &sc->sc_decap_ring;
-	struct wg_queue		*serial = &peer->p_decap_queue;
-	struct wg_tag		*t;
+	pkt->p_state = WG_PACKET_UNCRYPTED;
 
 	mtx_enter(&serial->q_mtx);
-	if (serial->q_list.ml_len < MAX_QUEUED_PKT) {
-		ml_enqueue(&serial->q_list, m);
+	if (serial->q_len < MAX_QUEUED_PKT) {
+		serial->q_len++;
+		SIMPLEQ_INSERT_TAIL(&serial->q_queue, pkt, p_serial);
 		mtx_leave(&serial->q_mtx);
 	} else {
 		mtx_leave(&serial->q_mtx);
-		m_freem(m);
+		m_freem(pkt->p_mbuf);
+		pool_put(&wg_packet_pool, pkt);
 		return ENOBUFS;
 	}
 
-	mtx_enter(&parallel->r_mtx);
-	if (parallel->r_tail - parallel->r_head < MAX_QUEUED_PKT) {
-		parallel->r_buf[parallel->r_tail & MAX_QUEUED_PKT_MASK] = m;
-		parallel->r_tail++;
-		mtx_leave(&parallel->r_mtx);
+	mtx_enter(&parallel->q_mtx);
+	if (parallel->q_len < MAX_QUEUED_PKT) {
+		parallel->q_len++;
+		SIMPLEQ_INSERT_TAIL(&parallel->q_queue, pkt, p_parallel);
+		mtx_leave(&parallel->q_mtx);
 	} else {
-		mtx_leave(&parallel->r_mtx);
-		t = wg_tag_get(m);
-		t->t_done = 1;
+		mtx_leave(&parallel->q_mtx);
+		pkt->p_state = WG_PACKET_DEAD;
 		return ENOBUFS;
 	}
 
@@ -1822,84 +1830,63 @@ wg_queue_in(struct wg_softc *sc, struct wg_peer *peer, struct mbuf *m)
 }
 
 void
+wg_queue_in(struct wg_softc *sc, struct wg_peer *peer, struct wg_packet *pkt)
+{
+	if (wg_queue_both(&sc->sc_decap_parallel, &peer->p_decap_serial, pkt) != 0)
+		counters_inc(sc->sc_if.if_counters, ifc_iqdrops);
+	task_add(wg_crypt_taskq, &sc->sc_decap);
+}
+
+void
 wg_queue_out(struct wg_softc *sc, struct wg_peer *peer)
 {
-	struct wg_ring		*parallel = &sc->sc_encap_ring;
-	struct wg_queue		*serial = &peer->p_encap_queue;
-	struct mbuf_list 	 ml, ml_free;
+	struct wg_packet	*pkt;
+	struct mbuf_list	 ml;
 	struct mbuf		*m;
-	struct wg_tag		*t;
-	int			 dropped;
 
-	/*
-	 * We delist all staged packets and then add them to the queues. This
-	 * can race with wg_qstart when called from wg_send_keepalive, however
-	 * wg_qstart will not race as it is serialised.
-	 */
+	if (noise_remote_ready(&peer->p_remote) != 0) {
+		wg_timers_event_want_initiation(&peer->p_timers);
+		return;
+	}
+
 	mq_delist(&peer->p_stage_queue, &ml);
-	ml_init(&ml_free);
 
 	while ((m = ml_dequeue(&ml)) != NULL) {
-		mtx_enter(&serial->q_mtx);
-		if (serial->q_list.ml_len < MAX_QUEUED_PKT) {
-			ml_enqueue(&serial->q_list, m);
-			mtx_leave(&serial->q_mtx);
-		} else {
-			mtx_leave(&serial->q_mtx);
-			ml_enqueue(&ml_free, m);
-			continue;
-		}
+		pkt = m->m_pkthdr.ph_cookie;
 
-		mtx_enter(&parallel->r_mtx);
-		if (parallel->r_tail - parallel->r_head < MAX_QUEUED_PKT) {
-			parallel->r_buf[parallel->r_tail & MAX_QUEUED_PKT_MASK] = m;
-			parallel->r_tail++;
-			mtx_leave(&parallel->r_mtx);
-		} else {
-			mtx_leave(&parallel->r_mtx);
-			t = wg_tag_get(m);
-			t->t_done = 1;
-		}
+		if (wg_queue_both(&sc->sc_encap_parallel, &peer->p_encap_serial, pkt) != 0)
+			counters_inc(sc->sc_if.if_counters, ifc_oqdrops);
 	}
 
-	if ((dropped = ml_purge(&ml_free)) > 0)
-		counters_add(sc->sc_if.if_counters, ifc_oqdrops, dropped);
+	task_add(wg_crypt_taskq, &sc->sc_encap);
 }
 
-struct mbuf *
-wg_ring_dequeue(struct wg_ring *r)
+struct wg_packet *
+wg_queue_serial_dequeue(struct wg_queue *serial)
 {
-	struct mbuf *m = NULL;
-	mtx_enter(&r->r_mtx);
-	if (r->r_head != r->r_tail) {
-		m = r->r_buf[r->r_head & MAX_QUEUED_PKT_MASK];
-		r->r_head++;
+	struct wg_packet *pkt = NULL;
+	mtx_enter(&serial->q_mtx);
+	if (serial->q_len > 0 && SIMPLEQ_FIRST(&serial->q_queue)->p_state != WG_PACKET_UNCRYPTED) {
+		serial->q_len--;
+		pkt = SIMPLEQ_FIRST(&serial->q_queue);
+		SIMPLEQ_REMOVE_HEAD(&serial->q_queue, p_serial);
 	}
-	mtx_leave(&r->r_mtx);
-	return m;
-}
-
-struct mbuf *
-wg_queue_dequeue(struct wg_queue *q, struct wg_tag **t)
-{
-	struct mbuf *m;
-	mtx_enter(&q->q_mtx);
-	if ((m = q->q_list.ml_head) != NULL && (*t = wg_tag_get(m))->t_done)
-		ml_dequeue(&q->q_list);
-	else
-		m = NULL;
-	mtx_leave(&q->q_mtx);
-	return m;
+	mtx_leave(&serial->q_mtx);
+	return pkt;
 }
 
-size_t
-wg_queue_len(struct wg_queue *q)
+struct wg_packet *
+wg_queue_parallel_dequeue(struct wg_queue *parallel)
 {
-	size_t len;
-	mtx_enter(&q->q_mtx);
-	len = q->q_list.ml_len;
-	mtx_leave(&q->q_mtx);
-	return len;
+	struct wg_packet *pkt = NULL;
+	mtx_enter(&parallel->q_mtx);
+	if (parallel->q_len > 0) {
+		parallel->q_len--;
+		pkt = SIMPLEQ_FIRST(&parallel->q_queue);
+		SIMPLEQ_REMOVE_HEAD(&parallel->q_queue, p_parallel);
+	}
+	mtx_leave(&parallel->q_mtx);
+	return pkt;
 }
 
 struct noise_remote *
@@ -1993,47 +1980,53 @@ wg_input(void *_sc, struct mbuf *m, struct ip *ip, struct ip6_hdr *ip6,
 {
 	struct wg_pkt_data	*data;
 	struct noise_remote	*remote;
-	struct wg_tag		*t;
+	struct wg_packet	*pkt;
+	struct wg_peer		*peer;
 	struct wg_softc		*sc = _sc;
 	struct udphdr		*uh = _uh;
 
 	NET_ASSERT_LOCKED();
 
-	if ((t = wg_tag_get(m)) == NULL) {
+	if ((pkt = pool_get(&wg_packet_pool, PR_NOWAIT)) == NULL) {
+		counters_inc(sc->sc_if.if_counters, ifc_iqdrops);
 		m_freem(m);
 		return NULL;
 	}
 
 	if (ip != NULL) {
-		t->t_endpoint.e_remote.r_sa.sa_len = sizeof(struct sockaddr_in);
-		t->t_endpoint.e_remote.r_sa.sa_family = AF_INET;
-		t->t_endpoint.e_remote.r_sin.sin_port = uh->uh_sport;
-		t->t_endpoint.e_remote.r_sin.sin_addr = ip->ip_src;
-		t->t_endpoint.e_local.l_in = ip->ip_dst;
+		pkt->p_endpoint.e_remote.r_sa.sa_len = sizeof(struct sockaddr_in);
+		pkt->p_endpoint.e_remote.r_sa.sa_family = AF_INET;
+		pkt->p_endpoint.e_remote.r_sin.sin_port = uh->uh_sport;
+		pkt->p_endpoint.e_remote.r_sin.sin_addr = ip->ip_src;
+		pkt->p_endpoint.e_local.l_in = ip->ip_dst;
 #ifdef INET6
 	} else if (ip6 != NULL) {
-		t->t_endpoint.e_remote.r_sa.sa_len = sizeof(struct sockaddr_in6);
-		t->t_endpoint.e_remote.r_sa.sa_family = AF_INET6;
-		t->t_endpoint.e_remote.r_sin6.sin6_port = uh->uh_sport;
-		t->t_endpoint.e_remote.r_sin6.sin6_addr = ip6->ip6_src;
-		t->t_endpoint.e_local.l_in6 = ip6->ip6_dst;
+		pkt->p_endpoint.e_remote.r_sa.sa_len = sizeof(struct sockaddr_in6);
+		pkt->p_endpoint.e_remote.r_sa.sa_family = AF_INET6;
+		pkt->p_endpoint.e_remote.r_sin6.sin6_port = uh->uh_sport;
+		pkt->p_endpoint.e_remote.r_sin6.sin6_addr = ip6->ip6_src;
+		pkt->p_endpoint.e_local.l_in6 = ip6->ip6_dst;
 #endif
 	} else {
-		m_freem(m);
-		return NULL;
+		counters_inc(sc->sc_if.if_counters, ifc_ierrors);
+		goto error_mbuf;
 	}
 
 	/* m has a IP/IPv6 header of hlen length, we don't need it anymore. */
 	m_adj(m, hlen);
 
+	/* TODO rework to not do a pullup(pkthdr.len) */
 	/*
 	 * Ensure mbuf is contiguous over full length of packet. This is done
 	 * os we can directly read the handshake values in wg_handshake, and so
 	 * we can decrypt a transport packet by passing a single buffer to
 	 * noise_remote_decrypt in wg_decap.
 	 */
-	if ((m = m_pullup(m, m->m_pkthdr.len)) == NULL)
-		return NULL;
+	if ((m = m_pullup(m, m->m_pkthdr.len)) == NULL) {
+		counters_inc(sc->sc_if.if_counters, ifc_ierrors);
+		goto error_packet;
+	}
+		pkt->p_mbuf = m;
 
 	if ((m->m_pkthdr.len == sizeof(struct wg_pkt_initiation) &&
 		*mtod(m, uint32_t *) == WG_PKT_INITIATION) ||
@@ -2042,35 +2035,37 @@ wg_input(void *_sc, struct mbuf *m, struct ip *ip, struct ip6_hdr *ip6,
 	    (m->m_pkthdr.len == sizeof(struct wg_pkt_cookie) &&
 		*mtod(m, uint32_t *) == WG_PKT_COOKIE)) {
 
-		if (mq_enqueue(&sc->sc_handshake_queue, m) != 0)
+		pkt->p_mbuf = m;
+		m->m_pkthdr.ph_cookie = pkt;
+		if (mq_enqueue(&sc->sc_handshake_queue, m) != 0) {
+			counters_inc(sc->sc_if.if_counters, ifc_iqdrops);
 			DPRINTF(sc, "Dropping handshake packet\n");
+			goto error_mbuf;
+		}
 		task_add(wg_handshake_taskq, &sc->sc_handshake);
-
 	} else if (m->m_pkthdr.len >= sizeof(struct wg_pkt_data) +
 	    NOISE_AUTHTAG_LEN && *mtod(m, uint32_t *) == WG_PKT_DATA) {
 
 		data = mtod(m, struct wg_pkt_data *);
 
-		if ((remote = wg_index_get(sc, data->r_idx)) != NULL) {
-			t->t_peer = CONTAINER_OF(remote, struct wg_peer,
-			    p_remote);
-			t->t_mbuf = NULL;
-			t->t_done = 0;
+		if ((remote = wg_index_get(sc, data->r_idx)) == NULL)
+			goto error_mbuf;
 
-			if (wg_queue_in(sc, t->t_peer, m) != 0)
-				counters_inc(sc->sc_if.if_counters,
-				    ifc_iqdrops);
-			task_add(wg_crypt_taskq, &sc->sc_decap);
-		} else {
-			counters_inc(sc->sc_if.if_counters, ifc_ierrors);
-			m_freem(m);
-		}
+		peer = CONTAINER_OF(remote, struct wg_peer, p_remote);
+		pkt->p_peer = peer;
+		pkt->p_mbuf = m;
+		wg_queue_in(sc, peer, pkt);
 	} else {
 		counters_inc(sc->sc_if.if_counters, ifc_ierrors);
-		m_freem(m);
+		goto error_mbuf;
 	}
 
 	return NULL;
+error_mbuf:
+	m_freem(m);
+error_packet:
+	pool_put(&wg_packet_pool, pkt);
+	return NULL;
 }
 
 void
@@ -2079,20 +2074,28 @@ wg_qstart(struct ifqueue *ifq)
 	struct ifnet		*ifp = ifq->ifq_if;
 	struct wg_softc		*sc = ifp->if_softc;
 	struct wg_peer		*peer;
+	struct wg_packet	*pkt;
 	struct wg_tag		*t;
 	struct mbuf		*m;
 	SLIST_HEAD(,wg_peer)	 start_list;
 
 	SLIST_INIT(&start_list);
 
-	/*
-	 * We should be OK to modify p_start_list, p_start_onlist in this
-	 * function as there should only be one ifp->if_qstart invoked at a
-	 * time.
-	 */
 	while ((m = ifq_dequeue(ifq)) != NULL) {
+		if ((pkt = pool_get(&wg_packet_pool, PR_NOWAIT)) == NULL) {
+			counters_inc(sc->sc_if.if_counters, ifc_iqdrops);
+			m_freem(m);
+			continue;
+		}
+
 		t = wg_tag_get(m);
 		peer = t->t_peer;
+
+		pkt->p_mbuf = m;
+		pkt->p_peer = peer;
+		pkt->p_mtu = t->t_mtu;
+		m->m_pkthdr.ph_cookie = pkt;
+
 		if (mq_push(&peer->p_stage_queue, m) != 0)
 			counters_inc(ifp->if_counters, ifc_oqdrops);
 		if (!peer->p_start_onlist) {
@@ -2101,13 +2104,9 @@ wg_qstart(struct ifqueue *ifq)
 		}
 	}
 	SLIST_FOREACH(peer, &start_list, p_start_list) {
-		if (noise_remote_ready(&peer->p_remote) == 0)
-			wg_queue_out(sc, peer);
-		else
-			wg_timers_event_want_initiation(&peer->p_timers);
+		wg_queue_out(peer->p_sc, peer);
 		peer->p_start_onlist = 0;
 	}
-	task_add(wg_crypt_taskq, &sc->sc_encap);
 }
 
 int
@@ -2121,11 +2120,6 @@ wg_output(struct ifnet *ifp, struct mbuf *m, struct sockaddr *sa,
 
 	NET_ASSERT_LOCKED();
 
-	if ((t = wg_tag_get(m)) == NULL) {
-		ret = ENOBUFS;
-		goto error;
-	}
-
 	m->m_pkthdr.ph_family = sa->sa_family;
 	if (sa->sa_family == AF_INET) {
 		peer = wg_aip_lookup(sc->sc_aip4,
@@ -2166,6 +2160,19 @@ wg_output(struct ifnet *ifp, struct mbuf *m, struct sockaddr *sa,
 	}
 
 	/*
+	 * We need to save a reference to the peer, and since the ifq system
+	 * modifies the ph_cookie, we can't use that. Instead, add a new
+	 * tag to save to. This is consumed purely in wg_qstart.
+	 */
+	if ((t = wg_tag_get(m)) == NULL) {
+		ret = ENOBUFS;
+		goto error;
+	}
+
+	t->t_peer = peer;
+	t->t_mtu = rt->rt_mtu > 0 ? rt->rt_mtu : ifp->if_mtu;
+
+	/*
 	 * As we hold a reference to peer in the mbuf, we can't handle a
 	 * delayed packet without doing some refcnting. If a peer is removed
 	 * while a delayed holds a reference, bad things will happen. For the
@@ -2178,11 +2185,6 @@ wg_output(struct ifnet *ifp, struct mbuf *m, struct sockaddr *sa,
 		goto error;
 	}
 
-	t->t_peer = peer;
-	t->t_mbuf = NULL;
-	t->t_done = 0;
-	t->t_mtu = ifp->if_mtu;
-
 	/*
 	 * We still have an issue with ifq that will count a packet that gets
 	 * dropped in wg_qstart, or not encrypted. These get counted as
@@ -2530,10 +2532,8 @@ wg_up(struct wg_softc *sc)
 		 */
 		ret = wg_bind(sc, &sc->sc_udp_port, &sc->sc_udp_rtable);
 		if (ret == 0) {
-			TAILQ_FOREACH(peer, &sc->sc_peer_seq, p_seq_entry) {
+			TAILQ_FOREACH(peer, &sc->sc_peer_seq, p_seq_entry)
 				wg_timers_enable(&peer->p_timers);
-				wg_queue_out(sc, peer);
-			}
 		}
 		rw_exit_write(&sc->sc_lock);
 
@@ -2654,10 +2654,8 @@ wg_clone_create(struct if_clone *ifc, int unit)
 	task_set(&sc->sc_encap, wg_encap_worker, sc);
 	task_set(&sc->sc_decap, wg_decap_worker, sc);
 
-	bzero(&sc->sc_encap_ring, sizeof(sc->sc_encap_ring));
-	mtx_init(&sc->sc_encap_ring.r_mtx, IPL_NET);
-	bzero(&sc->sc_decap_ring, sizeof(sc->sc_decap_ring));
-	mtx_init(&sc->sc_decap_ring.r_mtx, IPL_NET);
+	wg_queue_init(&sc->sc_encap_parallel);
+	wg_queue_init(&sc->sc_decap_parallel);
 
 	/* We've setup the softc, now we can setup the ifnet */
 	ifp = &sc->sc_if;
@@ -2703,6 +2701,7 @@ ret_01:
 ret_00:
 	return ENOBUFS;
 }
+
 int
 wg_clone_destroy(struct ifnet *ifp)
 {
@@ -2755,6 +2754,8 @@ wgattach(int nwg)
 			IPL_NET, 0, "wgaip", NULL);
 	pool_init(&wg_peer_pool, sizeof(struct wg_peer), 0,
 			IPL_NET, 0, "wgpeer", NULL);
+	pool_init(&wg_packet_pool, sizeof(struct wg_packet), 0,
+			IPL_NET, 0, "wgpacket", NULL);
 	pool_init(&wg_ratelimit_pool, sizeof(struct ratelimit_entry), 0,
 			IPL_NET, 0, "wgratelimit", NULL);
 }