1// SPDX-License-Identifier: GPL-2.0
2/*
3 * Copyright (C) 2015-2019 Jason A. Donenfeld <Jason@zx2c4.com>. All Rights Reserved.
4 */
5
6#include "queueing.h"
7#include <linux/skb_array.h>
8
9struct multicore_worker __percpu *
10wg_packet_percpu_multicore_worker_alloc(work_func_t function, void *ptr)
11{
12	int cpu;
13	struct multicore_worker __percpu *worker = alloc_percpu(struct multicore_worker);
14
15	if (!worker)
16		return NULL;
17
18	for_each_possible_cpu(cpu) {
19		per_cpu_ptr(worker, cpu)->ptr = ptr;
20		INIT_WORK(&per_cpu_ptr(worker, cpu)->work, function);
21	}
22	return worker;
23}
24
25int wg_packet_queue_init(struct crypt_queue *queue, work_func_t function,
26			 unsigned int len)
27{
28	int ret;
29
30	memset(queue, 0, sizeof(*queue));
31	queue->last_cpu = -1;
32	ret = ptr_ring_init(&queue->ring, len, GFP_KERNEL);
33	if (ret)
34		return ret;
35	queue->worker = wg_packet_percpu_multicore_worker_alloc(function, queue);
36	if (!queue->worker) {
37		ptr_ring_cleanup(&queue->ring, NULL);
38		return -ENOMEM;
39	}
40	return 0;
41}
42
43void wg_packet_queue_free(struct crypt_queue *queue, bool purge)
44{
45	free_percpu(queue->worker);
46	WARN_ON(!purge && !__ptr_ring_empty(&queue->ring));
47	ptr_ring_cleanup(&queue->ring, purge ? __skb_array_destroy_skb : NULL);
48}
49
50#define NEXT(skb) ((skb)->prev)
51#define STUB(queue) ((struct sk_buff *)&queue->empty)
52
53void wg_prev_queue_init(struct prev_queue *queue)
54{
55	NEXT(STUB(queue)) = NULL;
56	queue->head = queue->tail = STUB(queue);
57	queue->peeked = NULL;
58	atomic_set(&queue->count, 0);
59	BUILD_BUG_ON(
60		offsetof(struct sk_buff, next) != offsetof(struct prev_queue, empty.next) -
61							offsetof(struct prev_queue, empty) ||
62		offsetof(struct sk_buff, prev) != offsetof(struct prev_queue, empty.prev) -
63							 offsetof(struct prev_queue, empty));
64}
65
66static void __wg_prev_queue_enqueue(struct prev_queue *queue, struct sk_buff *skb)
67{
68	WRITE_ONCE(NEXT(skb), NULL);
69	WRITE_ONCE(NEXT(xchg_release(&queue->head, skb)), skb);
70}
71
72bool wg_prev_queue_enqueue(struct prev_queue *queue, struct sk_buff *skb)
73{
74	if (!atomic_add_unless(&queue->count, 1, MAX_QUEUED_PACKETS))
75		return false;
76	__wg_prev_queue_enqueue(queue, skb);
77	return true;
78}
79
80struct sk_buff *wg_prev_queue_dequeue(struct prev_queue *queue)
81{
82	struct sk_buff *tail = queue->tail, *next = smp_load_acquire(&NEXT(tail));
83
84	if (tail == STUB(queue)) {
85		if (!next)
86			return NULL;
87		queue->tail = next;
88		tail = next;
89		next = smp_load_acquire(&NEXT(next));
90	}
91	if (next) {
92		queue->tail = next;
93		atomic_dec(&queue->count);
94		return tail;
95	}
96	if (tail != READ_ONCE(queue->head))
97		return NULL;
98	__wg_prev_queue_enqueue(queue, STUB(queue));
99	next = smp_load_acquire(&NEXT(tail));
100	if (next) {
101		queue->tail = next;
102		atomic_dec(&queue->count);
103		return tail;
104	}
105	return NULL;
106}
107
108#undef NEXT
109#undef STUB
110