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 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 ret = ptr_ring_init(&queue->ring, len, GFP_KERNEL); 32 if (ret) 33 return ret; 34 queue->worker = wg_packet_percpu_multicore_worker_alloc(function, queue); 35 if (!queue->worker) { 36 ptr_ring_cleanup(&queue->ring, NULL); 37 return -ENOMEM; 38 } 39 return 0; 40 } 41 42 void wg_packet_queue_free(struct crypt_queue *queue, bool purge) 43 { 44 free_percpu(queue->worker); 45 WARN_ON(!purge && !__ptr_ring_empty(&queue->ring)); 46 ptr_ring_cleanup(&queue->ring, purge ? __skb_array_destroy_skb : NULL); 47 } 48 49 #define NEXT(skb) ((skb)->prev) 50 #define STUB(queue) ((struct sk_buff *)&queue->empty) 51 52 void wg_prev_queue_init(struct prev_queue *queue) 53 { 54 NEXT(STUB(queue)) = NULL; 55 queue->head = queue->tail = STUB(queue); 56 queue->peeked = NULL; 57 atomic_set(&queue->count, 0); 58 BUILD_BUG_ON( 59 offsetof(struct sk_buff, next) != offsetof(struct prev_queue, empty.next) - 60 offsetof(struct prev_queue, empty) || 61 offsetof(struct sk_buff, prev) != offsetof(struct prev_queue, empty.prev) - 62 offsetof(struct prev_queue, empty)); 63 } 64 65 static void __wg_prev_queue_enqueue(struct prev_queue *queue, struct sk_buff *skb) 66 { 67 WRITE_ONCE(NEXT(skb), NULL); 68 WRITE_ONCE(NEXT(xchg_release(&queue->head, skb)), skb); 69 } 70 71 bool wg_prev_queue_enqueue(struct prev_queue *queue, struct sk_buff *skb) 72 { 73 if (!atomic_add_unless(&queue->count, 1, MAX_QUEUED_PACKETS)) 74 return false; 75 __wg_prev_queue_enqueue(queue, skb); 76 return true; 77 } 78 79 struct sk_buff *wg_prev_queue_dequeue(struct prev_queue *queue) 80 { 81 struct sk_buff *tail = queue->tail, *next = smp_load_acquire(&NEXT(tail)); 82 83 if (tail == STUB(queue)) { 84 if (!next) 85 return NULL; 86 queue->tail = next; 87 tail = next; 88 next = smp_load_acquire(&NEXT(next)); 89 } 90 if (next) { 91 queue->tail = next; 92 atomic_dec(&queue->count); 93 return tail; 94 } 95 if (tail != READ_ONCE(queue->head)) 96 return NULL; 97 __wg_prev_queue_enqueue(queue, STUB(queue)); 98 next = smp_load_acquire(&NEXT(tail)); 99 if (next) { 100 queue->tail = next; 101 atomic_dec(&queue->count); 102 return tail; 103 } 104 return NULL; 105 } 106 107 #undef NEXT 108 #undef STUB 109