| 1 | |
| 2 | |
| 3 | * Copyright (C) 2015-2019 Jason A. Donenfeld <[email protected]>. All Rights Reserved.
|
| 4 |
|
| 5 |
|
| 6 | #include "queueing.h"
|
| 7 | #include <linux/skb_array.h>
|
| 8 |
|
| 9 | struct multicore_worker __percpu *
|
| 10 | wg_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 |
|
| 25 | int 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 |
|
| 43 | void 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 |
|
| 53 | void 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 |
|
| 66 | static 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 |
|
| 72 | bool 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 |
|
| 80 | struct 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
|