WIP4

6 files changed, 107 insertions, 106 deletions

diff --git a/src/device.h b/src/device.h
index fae3845..218546e 100644
--- a/src/device.h
+++ b/src/device.h
@@ -17,7 +17,6 @@
 #include <linux/workqueue.h>
 #include <linux/mutex.h>
 #include <linux/net.h>
-//#include <linux/ptr_ring.h>
 
 struct wireguard_device;
 
@@ -27,9 +26,7 @@ struct multicore_worker {
 };
 
 struct crypt_queue {
-	//struct ptr_ring ring;
 	struct ck_ring ring;
-	struct ck_ring_buffer ring_buffer;
 	union {
 		struct {
 			struct multicore_worker __percpu *worker;
diff --git a/src/mpmc_ring.h b/src/mpmc_ring.h
index 8fd6d8f..9c46007 100644
--- a/src/mpmc_ring.h
+++ b/src/mpmc_ring.h
@@ -69,14 +69,23 @@
     return z; }
 */
 __always_inline static
-bool ck_pr_cas_uint_value(atomic_t *target, uint old, uint new, uint *v) {
-	uint prev = atomic_cmpxchg(target, old, new);
-	*v = new;
-	return prev != old;
+bool ck_pr_cas_uint(atomic_t *target, uint old, uint new)
+{
+	uint prev;
+	prev = atomic_cmpxchg(target, old, new);
+	//pr_err("cas(): old: %d, new: %d, prev: %d", old, new, prev);
+	return prev == old;
 }
 
-/* http://concurrencykit.org/doc/ck_pr_cas.html */
-#define ck_pr_cas_uint(t, old, new) atomic_cmpxchg(t, old, new) != old
+__always_inline static
+bool ck_pr_cas_uint_value(atomic_t *target, uint old, uint new, uint *v)
+{
+	bool ret;
+	//pr_err("cas_value(): %d\n", *v);
+	ret = ck_pr_cas_uint(target, old, new);
+	WRITE_ONCE(*v, ret ? old : new);
+	return ret;
+}
 
 /* http://concurrencykit.org/doc/ck_pr_store.html */
 // TODO: compiler barrier?
@@ -93,34 +102,33 @@ struct ck_ring {
 	atomic_t p_head;
 	unsigned int size ____cacheline_aligned_in_smp;
 	unsigned int mask;
+	void **queue;
 };
 typedef struct ck_ring ck_ring_t;
 
-struct ck_ring_buffer {
-	void *value;
-};
-typedef struct ck_ring_buffer ck_ring_buffer_t;
-
-CK_CC_INLINE static void
-ck_ring_init(struct ck_ring *ring, unsigned int size)
+static inline int ck_ring_init(struct ck_ring *ring, uint size, gfp_t gfp)
 {
 	ring->size = size;
 	ring->mask = size - 1;
 	atomic_set(&ring->p_tail, 0);
 	atomic_set(&ring->p_head, 0); // TODO: barrier?
 	atomic_set(&ring->c_head, 0);
-	return;
+
+	ring->queue = kcalloc(size, sizeof(void *), gfp);
+	if (!ring->queue)
+		return -ENOMEM;
+
+	memset(ring->queue, 0x77, sizeof(void *) * size);
+	return 0;
 }
 
 CK_CC_FORCE_INLINE static bool
-_ck_ring_enqueue_mp(struct ck_ring *ring,
-    void *buffer,
-    const void *entry,
-    unsigned int ts,
+_ck_ring_enqueue_mp(struct ck_ring *ring, const void *entry, unsigned int ts,
     unsigned int *size)
 {
 	const unsigned int mask = ring->mask;
 	unsigned int producer, consumer, delta;
+	void *buffer;
 	bool r = true;
 
 	producer = ck_pr_load_uint(&ring->p_head);
@@ -174,7 +182,8 @@ _ck_ring_enqueue_mp(struct ck_ring *ring,
 		}
 	}
 
-	buffer = (char *)buffer + ts * (producer & mask);
+	buffer = (char *)ring->queue + ts * (producer & mask);
+	//pr_err("memcpy(%p, %p, %u)", buffer, entry, ts);
 	memcpy(buffer, entry, ts);
 
 	/*
@@ -199,28 +208,24 @@ leave:
 }
 
 CK_CC_FORCE_INLINE static bool
-_ck_ring_enqueue_mp_size(struct ck_ring *ring,
-    void *buffer,
-    const void *entry,
-    unsigned int ts,
-    unsigned int *size)
+_ck_ring_enqueue_mp_size(struct ck_ring *ring, const void *entry,
+    unsigned int ts, unsigned int *size)
 {
 	unsigned int sz;
 	bool r;
 
-	r = _ck_ring_enqueue_mp(ring, buffer, entry, ts, &sz);
+	r = _ck_ring_enqueue_mp(ring, entry, ts, &sz);
 	*size = sz;
 	return r;
 }
 
 CK_CC_FORCE_INLINE static bool
 _ck_ring_trydequeue_mc(struct ck_ring *ring,
-    const void *buffer,
-    void *data,
-    unsigned int size)
+    void *data, unsigned int size)
 {
 	const unsigned int mask = ring->mask;
 	unsigned int consumer, producer;
+	const void *buffer;
 
 	consumer = ck_pr_load_uint(&ring->c_head);
 	ck_pr_fence_load();
@@ -231,7 +236,7 @@ _ck_ring_trydequeue_mc(struct ck_ring *ring,
 
 	ck_pr_fence_load();
 
-	buffer = (const char *)buffer + size * (consumer & mask);
+	buffer = (const char *)ring->queue + size * (consumer & mask);
 	memcpy(data, buffer, size);
 
 	ck_pr_fence_store_atomic();
@@ -240,9 +245,7 @@ _ck_ring_trydequeue_mc(struct ck_ring *ring,
 
 CK_CC_FORCE_INLINE static bool
 _ck_ring_dequeue_mc(struct ck_ring *ring,
-    const void *buffer,
-    void *data,
-    unsigned int ts)
+    void *data, unsigned int ts)
 {
 	const unsigned int mask = ring->mask;
 	unsigned int consumer, producer;
@@ -264,7 +267,7 @@ _ck_ring_dequeue_mc(struct ck_ring *ring,
 
 		ck_pr_fence_load();
 
-		target = (const char *)buffer + ts * (consumer & mask);
+		target = (const char *)ring->queue + ts * (consumer & mask);
 		memcpy(data, target, ts);
 
 		/* Serialize load with respect to head update. */
@@ -277,50 +280,83 @@ _ck_ring_dequeue_mc(struct ck_ring *ring,
 	return true;
 }
 
+static __always_inline bool mpmc_ring_empty(struct ck_ring *ring)
+{
+	uint producer, consumer;
+
+	consumer = ck_pr_load_uint(&ring->c_head);
+	ck_pr_fence_load();
+	producer = ck_pr_load_uint(&ring->p_tail);
+
+	ck_pr_fence_load();
+
+	return producer == consumer;
+}
+
+static __always_inline void mpmc_ring_cleanup(struct ck_ring *ring)
+{
+	kfree(ring->queue);
+}
+
+static __always_inline bool mpmc_ptr_ring_peek(struct ck_ring *ring, void *data,
+		uint size)
+{
+	uint producer, consumer;
+	const unsigned int mask = ring->mask;
+	void *buffer;
+
+	consumer = ck_pr_load_uint(&ring->c_head);
+	ck_pr_fence_load();
+	producer = ck_pr_load_uint(&ring->p_tail);
+
+	ck_pr_fence_load();
+
+	if (unlikely(producer == consumer)) {
+		data = NULL;
+		return false;
+	}
+
+	buffer = (char *)ring->queue + size * (consumer & mask);
+	memcpy(data, buffer, size);
+
+	return true;
+}
+
+static __always_inline void mpmc_ptr_ring_discard(struct ck_ring *ring)
+{
+	smp_mb__before_atomic();
+	atomic_inc(&ring->c_head);
+	smp_mb__after_atomic();
+}
+
 /*
  * The ck_ring_*_mpmc namespace is the public interface for interacting with a
  * ring buffer containing pointers. Correctness is provided for any number of
  * producers and consumers.
  */
 CK_CC_INLINE static bool
-ck_ring_enqueue_mpmc(struct ck_ring *ring,
-    struct ck_ring_buffer *buffer,
-    const void *entry)
+ck_ring_enqueue_mpmc(struct ck_ring *ring, const void *entry)
 {
-
-	return _ck_ring_enqueue_mp(ring, buffer, &entry,
-	    sizeof(entry), NULL);
+	return _ck_ring_enqueue_mp(ring, &entry, sizeof(entry), NULL);
 }
 
 CK_CC_INLINE static bool
-ck_ring_enqueue_mpmc_size(struct ck_ring *ring,
-    struct ck_ring_buffer *buffer,
-    const void *entry,
+ck_ring_enqueue_mpmc_size(struct ck_ring *ring, const void *entry,
     unsigned int *size)
 {
-
-	return _ck_ring_enqueue_mp_size(ring, buffer, &entry,
-	    sizeof(entry), size);
+	return _ck_ring_enqueue_mp_size(ring, &entry, sizeof(entry), size);
 }
 
 CK_CC_INLINE static bool
-ck_ring_trydequeue_mpmc(struct ck_ring *ring,
-    const struct ck_ring_buffer *buffer,
-    void *data)
+ck_ring_trydequeue_mpmc(struct ck_ring *ring, void *data)
 {
-
-	return _ck_ring_trydequeue_mc(ring,
-	    buffer, (void **)data, sizeof(void *));
+	return _ck_ring_trydequeue_mc(ring, (void **)data, sizeof(void *));
 }
 
 CK_CC_INLINE static bool
-ck_ring_dequeue_mpmc(struct ck_ring *ring,
-    const struct ck_ring_buffer *buffer,
-    void *data)
+ck_ring_dequeue_mpmc(struct ck_ring *ring, void *data)
 {
-
-	return _ck_ring_dequeue_mc(ring, buffer, (void **)data,
-	    sizeof(void *));
+	return _ck_ring_dequeue_mc(ring, (void **)data, sizeof(void *));
 }
 
 #endif /* _WG_MPMC_RING_H */
diff --git a/src/queueing.c b/src/queueing.c
index 7617622..be0b492 100644
--- a/src/queueing.c
+++ b/src/queueing.c
@@ -22,14 +22,12 @@ struct multicore_worker __percpu *packet_alloc_percpu_multicore_worker(work_func
 
 int packet_queue_init(struct crypt_queue *queue, work_func_t function, bool multicore, unsigned int len)
 {
-	/*int ret;*/
+	int ret;
 
 	memset(queue, 0, sizeof(*queue));
-	/*ret = ptr_ring_init(&queue->ring, len, GFP_KERNEL);*/
-	/*if (ret)*/
-		/*return ret;*/
-	ck_ring_init(&queue->ring, len);
-	queue->ring_buffer.value = kcalloc(len, sizeof(void *), GFP_KERNEL);
+	ret = ck_ring_init(&queue->ring, len, GFP_KERNEL);
+	if (ret)
+		return ret;
 	if (multicore) {
 		queue->worker = packet_alloc_percpu_multicore_worker(function, queue);
 		if (!queue->worker)
@@ -43,11 +41,6 @@ void packet_queue_free(struct crypt_queue *queue, bool multicore)
 {
 	if (multicore)
 		free_percpu(queue->worker);
-
-	/* TODO: from the ck docs: It is possible for the function to return
-	 * false even if ring is non-empty. See also
-	 * http://concurrencykit.org/doc/ck_ring_trydequeue_spmc.html */
-	/*WARN_ON(!ptr_ring_empty_bh(&queue->ring));*/
-	/*ptr_ring_cleanup(&queue->ring, NULL);*/
-	kfree(queue->ring_buffer.value);
+	WARN_ON(!mpmc_ring_empty(&queue->ring));
+	mpmc_ring_cleanup(&queue->ring);
 }
diff --git a/src/queueing.h b/src/queueing.h
index e90744e..51d0b8e 100644
--- a/src/queueing.h
+++ b/src/queueing.h
@@ -120,12 +120,10 @@ static inline int queue_enqueue_per_device_and_peer(struct crypt_queue *device_q
 	int cpu;
 
 	atomic_set(&PACKET_CB(skb)->state, PACKET_STATE_UNCRYPTED);
-	//if (unlikely(ptr_ring_produce_bh(&peer_queue->ring, skb)))
-	if (unlikely(ck_ring_enqueue_mpmc(&peer_queue->ring, &peer_queue->ring_buffer, skb)))
+	if (unlikely(!ck_ring_enqueue_mpmc(&peer_queue->ring, skb)))
 		return -ENOSPC;
 	cpu = cpumask_next_online(next_cpu);
-	//if (unlikely(ptr_ring_produce_bh(&device_queue->ring, skb)))
-	if (unlikely(ck_ring_enqueue_mpmc(&device_queue->ring, &device_queue->ring_buffer, skb)))
+	if (unlikely(!ck_ring_enqueue_mpmc(&device_queue->ring, skb)))
 		return -EPIPE;
 	queue_work_on(cpu, wq, &per_cpu_ptr(device_queue->worker, cpu)->work);
 	return 0;
diff --git a/src/receive.c b/src/receive.c
index 5ab4171..3de698f 100644
--- a/src/receive.c
+++ b/src/receive.c
@@ -378,18 +378,8 @@ void packet_rx_worker(struct work_struct *work)
 	bool free;
 
 	local_bh_disable();
-	/*spin_lock_bh(&queue->ring.consumer_lock);*/
-	/*while ((skb = __ptr_ring_peek(&queue->ring)) != NULL && (state = atomic_read(&PACKET_CB(skb)->state)) != PACKET_STATE_UNCRYPTED) {*/
-		/*__ptr_ring_discard_one(&queue->ring);*/
-
-	// TODO: this is very wrong
-	while (ck_ring_dequeue_mpmc(&queue->ring, &queue->ring_buffer, &skb)) {
-		if ((state = atomic_read(&PACKET_CB(skb)->state)) == PACKET_STATE_UNCRYPTED) {
-			ck_ring_enqueue_mpmc(&queue->ring, &queue->ring_buffer, &skb); // ignores error
-			continue;
-		}
-
-
+	while (mpmc_ptr_ring_peek(&queue->ring, &skb, sizeof(struct sk_buff*)) && (state = atomic_read(&PACKET_CB(skb)->state)) != PACKET_STATE_UNCRYPTED) {
+		mpmc_ptr_ring_discard(&queue->ring);
 		peer = PACKET_PEER(skb);
 		keypair = PACKET_CB(skb)->keypair;
 		free = true;
@@ -415,7 +405,6 @@ next:
 		if (unlikely(free))
 			dev_kfree_skb(skb);
 	}
-	/*spin_unlock_bh(&queue->ring.consumer_lock);*/
 	local_bh_enable();
 }
 
@@ -425,8 +414,7 @@ void packet_decrypt_worker(struct work_struct *work)
 	struct sk_buff *skb;
 	bool have_simd = chacha20poly1305_init_simd();
 
-	/*while ((skb = ptr_ring_consume_bh(&queue->ring)) != NULL) {*/
-	while (ck_ring_dequeue_mpmc(&queue->ring, &queue->ring_buffer, &skb)) {
+	while (ck_ring_dequeue_mpmc(&queue->ring, &skb)) {
 		enum packet_state state = likely(skb_decrypt(skb, &PACKET_CB(skb)->keypair->receiving, have_simd)) ? PACKET_STATE_CRYPTED : PACKET_STATE_DEAD;
 
 		queue_enqueue_per_peer(&PACKET_PEER(skb)->rx_queue, skb, state);
diff --git a/src/send.c b/src/send.c
index 4c78a06..b4aff0e 100644
--- a/src/send.c
+++ b/src/send.c
@@ -223,17 +223,8 @@ void packet_tx_worker(struct work_struct *work)
 	struct sk_buff *first;
 	enum packet_state state;
 
-	/*spin_lock_bh(&queue->ring.consumer_lock);*/
-	/*while ((first = __ptr_ring_peek(&queue->ring)) != NULL && (state = atomic_read(&PACKET_CB(first)->state)) != PACKET_STATE_UNCRYPTED) {*/
-		/*__ptr_ring_discard_one(&queue->ring);*/
-
-	// TODO: this is very wrong
-	while (ck_ring_dequeue_mpmc(&queue->ring, &queue->ring_buffer, &first)) {
-		if ((state = atomic_read(&PACKET_CB(first)->state)) == PACKET_STATE_UNCRYPTED) {
-			ck_ring_enqueue_mpmc(&queue->ring, &queue->ring_buffer, &first); // ignores error
-			continue;
-		}
-
+	while (mpmc_ptr_ring_peek(&queue->ring, &first, sizeof(struct sk_buff*)) && (state = atomic_read(&PACKET_CB(first)->state)) != PACKET_STATE_UNCRYPTED) {
+		mpmc_ptr_ring_discard(&queue->ring);
 		peer = PACKET_PEER(first);
 		keypair = PACKET_CB(first)->keypair;
 
@@ -245,7 +236,6 @@ void packet_tx_worker(struct work_struct *work)
 		noise_keypair_put(keypair);
 		peer_put(peer);
 	}
-	/*spin_unlock_bh(&queue->ring.consumer_lock);*/
 }
 
 void packet_encrypt_worker(struct work_struct *work)
@@ -254,8 +244,7 @@ void packet_encrypt_worker(struct work_struct *work)
 	struct sk_buff *first, *skb, *next;
 	bool have_simd = chacha20poly1305_init_simd();
 
-	/*while ((first = ptr_ring_consume_bh(&queue->ring)) != NULL) {*/
-	while (ck_ring_dequeue_mpmc(&queue->ring, &queue->ring_buffer, &first)) {
+	while (ck_ring_dequeue_mpmc(&queue->ring, &first)) {
 		enum packet_state state = PACKET_STATE_CRYPTED;
 
 		skb_walk_null_queue_safe(first, skb, next) {