1e7096c13SJason A. Donenfeld // SPDX-License-Identifier: GPL-2.0
2e7096c13SJason A. Donenfeld /*
3e7096c13SJason A. Donenfeld * Copyright (C) 2015-2019 Jason A. Donenfeld <Jason@zx2c4.com>. All Rights Reserved.
4e7096c13SJason A. Donenfeld */
5e7096c13SJason A. Donenfeld
6e7096c13SJason A. Donenfeld #include "queueing.h"
7ec59f128SJason A. Donenfeld #include <linux/skb_array.h>
8e7096c13SJason A. Donenfeld
9e7096c13SJason A. Donenfeld struct multicore_worker __percpu *
wg_packet_percpu_multicore_worker_alloc(work_func_t function,void * ptr)10e7096c13SJason A. Donenfeld wg_packet_percpu_multicore_worker_alloc(work_func_t function, void *ptr)
11e7096c13SJason A. Donenfeld {
12e7096c13SJason A. Donenfeld int cpu;
138b5553acSJason A. Donenfeld struct multicore_worker __percpu *worker = alloc_percpu(struct multicore_worker);
14e7096c13SJason A. Donenfeld
15e7096c13SJason A. Donenfeld if (!worker)
16e7096c13SJason A. Donenfeld return NULL;
17e7096c13SJason A. Donenfeld
18e7096c13SJason A. Donenfeld for_each_possible_cpu(cpu) {
19e7096c13SJason A. Donenfeld per_cpu_ptr(worker, cpu)->ptr = ptr;
20e7096c13SJason A. Donenfeld INIT_WORK(&per_cpu_ptr(worker, cpu)->work, function);
21e7096c13SJason A. Donenfeld }
22e7096c13SJason A. Donenfeld return worker;
23e7096c13SJason A. Donenfeld }
24e7096c13SJason A. Donenfeld
wg_packet_queue_init(struct crypt_queue * queue,work_func_t function,unsigned int len)25e7096c13SJason A. Donenfeld int wg_packet_queue_init(struct crypt_queue *queue, work_func_t function,
268b5553acSJason A. Donenfeld unsigned int len)
27e7096c13SJason A. Donenfeld {
28e7096c13SJason A. Donenfeld int ret;
29e7096c13SJason A. Donenfeld
30e7096c13SJason A. Donenfeld memset(queue, 0, sizeof(*queue));
31*7387943fSJason A. Donenfeld queue->last_cpu = -1;
32e7096c13SJason A. Donenfeld ret = ptr_ring_init(&queue->ring, len, GFP_KERNEL);
33e7096c13SJason A. Donenfeld if (ret)
34e7096c13SJason A. Donenfeld return ret;
358b5553acSJason A. Donenfeld queue->worker = wg_packet_percpu_multicore_worker_alloc(function, queue);
36130c5860SJason A. Donenfeld if (!queue->worker) {
37130c5860SJason A. Donenfeld ptr_ring_cleanup(&queue->ring, NULL);
38e7096c13SJason A. Donenfeld return -ENOMEM;
39130c5860SJason A. Donenfeld }
40e7096c13SJason A. Donenfeld return 0;
41e7096c13SJason A. Donenfeld }
42e7096c13SJason A. Donenfeld
wg_packet_queue_free(struct crypt_queue * queue,bool purge)43886fcee9SJason A. Donenfeld void wg_packet_queue_free(struct crypt_queue *queue, bool purge)
44e7096c13SJason A. Donenfeld {
45e7096c13SJason A. Donenfeld free_percpu(queue->worker);
46886fcee9SJason A. Donenfeld WARN_ON(!purge && !__ptr_ring_empty(&queue->ring));
47ec59f128SJason A. Donenfeld ptr_ring_cleanup(&queue->ring, purge ? __skb_array_destroy_skb : NULL);
48e7096c13SJason A. Donenfeld }
498b5553acSJason A. Donenfeld
508b5553acSJason A. Donenfeld #define NEXT(skb) ((skb)->prev)
518b5553acSJason A. Donenfeld #define STUB(queue) ((struct sk_buff *)&queue->empty)
528b5553acSJason A. Donenfeld
wg_prev_queue_init(struct prev_queue * queue)538b5553acSJason A. Donenfeld void wg_prev_queue_init(struct prev_queue *queue)
548b5553acSJason A. Donenfeld {
558b5553acSJason A. Donenfeld NEXT(STUB(queue)) = NULL;
568b5553acSJason A. Donenfeld queue->head = queue->tail = STUB(queue);
578b5553acSJason A. Donenfeld queue->peeked = NULL;
588b5553acSJason A. Donenfeld atomic_set(&queue->count, 0);
598b5553acSJason A. Donenfeld BUILD_BUG_ON(
608b5553acSJason A. Donenfeld offsetof(struct sk_buff, next) != offsetof(struct prev_queue, empty.next) -
618b5553acSJason A. Donenfeld offsetof(struct prev_queue, empty) ||
628b5553acSJason A. Donenfeld offsetof(struct sk_buff, prev) != offsetof(struct prev_queue, empty.prev) -
638b5553acSJason A. Donenfeld offsetof(struct prev_queue, empty));
648b5553acSJason A. Donenfeld }
658b5553acSJason A. Donenfeld
__wg_prev_queue_enqueue(struct prev_queue * queue,struct sk_buff * skb)668b5553acSJason A. Donenfeld static void __wg_prev_queue_enqueue(struct prev_queue *queue, struct sk_buff *skb)
678b5553acSJason A. Donenfeld {
688b5553acSJason A. Donenfeld WRITE_ONCE(NEXT(skb), NULL);
698b5553acSJason A. Donenfeld WRITE_ONCE(NEXT(xchg_release(&queue->head, skb)), skb);
708b5553acSJason A. Donenfeld }
718b5553acSJason A. Donenfeld
wg_prev_queue_enqueue(struct prev_queue * queue,struct sk_buff * skb)728b5553acSJason A. Donenfeld bool wg_prev_queue_enqueue(struct prev_queue *queue, struct sk_buff *skb)
738b5553acSJason A. Donenfeld {
748b5553acSJason A. Donenfeld if (!atomic_add_unless(&queue->count, 1, MAX_QUEUED_PACKETS))
758b5553acSJason A. Donenfeld return false;
768b5553acSJason A. Donenfeld __wg_prev_queue_enqueue(queue, skb);
778b5553acSJason A. Donenfeld return true;
788b5553acSJason A. Donenfeld }
798b5553acSJason A. Donenfeld
wg_prev_queue_dequeue(struct prev_queue * queue)808b5553acSJason A. Donenfeld struct sk_buff *wg_prev_queue_dequeue(struct prev_queue *queue)
818b5553acSJason A. Donenfeld {
828b5553acSJason A. Donenfeld struct sk_buff *tail = queue->tail, *next = smp_load_acquire(&NEXT(tail));
838b5553acSJason A. Donenfeld
848b5553acSJason A. Donenfeld if (tail == STUB(queue)) {
858b5553acSJason A. Donenfeld if (!next)
868b5553acSJason A. Donenfeld return NULL;
878b5553acSJason A. Donenfeld queue->tail = next;
888b5553acSJason A. Donenfeld tail = next;
898b5553acSJason A. Donenfeld next = smp_load_acquire(&NEXT(next));
908b5553acSJason A. Donenfeld }
918b5553acSJason A. Donenfeld if (next) {
928b5553acSJason A. Donenfeld queue->tail = next;
938b5553acSJason A. Donenfeld atomic_dec(&queue->count);
948b5553acSJason A. Donenfeld return tail;
958b5553acSJason A. Donenfeld }
968b5553acSJason A. Donenfeld if (tail != READ_ONCE(queue->head))
978b5553acSJason A. Donenfeld return NULL;
988b5553acSJason A. Donenfeld __wg_prev_queue_enqueue(queue, STUB(queue));
998b5553acSJason A. Donenfeld next = smp_load_acquire(&NEXT(tail));
1008b5553acSJason A. Donenfeld if (next) {
1018b5553acSJason A. Donenfeld queue->tail = next;
1028b5553acSJason A. Donenfeld atomic_dec(&queue->count);
1038b5553acSJason A. Donenfeld return tail;
1048b5553acSJason A. Donenfeld }
1058b5553acSJason A. Donenfeld return NULL;
1068b5553acSJason A. Donenfeld }
1078b5553acSJason A. Donenfeld
1088b5553acSJason A. Donenfeld #undef NEXT
1098b5553acSJason A. Donenfeld #undef STUB
110