voron

commit 071ae4824672ef44712f86c40e81edad08701200
parent 73395a92ba65e1bffb4589714336a59b8a544573
Author: sin <sin@2f30.org>
Date:   Sun, 21 Jul 2013 21:07:52 +0100

Add kfifo

Diffstat:
A
include/kfifo.h
 | 
9
+++++++++
A
kernel/kfifo.c
 | 
82
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

2 files changed, 91 insertions(+), 0 deletions(-)
diff --git a/include/kfifo.h b/include/kfifo.h
@@ -0,0 +1,9 @@
+#ifndef __KFIFO_H
+#define __KFIFO_H
+
+struct kfifo *init_kfifo(size_t cap);
+void free_kfifo(struct kfifo *kfifo);
+int enqueue_kfifo(struct kfifo *kfifo, void *data, size_t siz);
+int dequeue_kfifo(struct kfifo *kfifo, void *data, size_t siz);
+
+#endif
diff --git a/kernel/kfifo.c b/kernel/kfifo.c
@@ -0,0 +1,82 @@
+#include <kernel.h>
+#include <alloc.h>
+#include <spinlock.h>
+#include "kfifo.h"
+
+struct kfifo {
+	uint8_t *buf;
+	size_t siz;
+	size_t cap;
+};
+
+static spinlock_t kfifo_lock = SPINLOCK_INIT;
+
+struct kfifo *
+init_kfifo(size_t cap)
+{
+	struct kfifo *kfifo;
+
+	kfifo = kmalloc(sizeof(*kfifo));
+	if (!kfifo)
+		return NULL;
+	if (cap > 0) {
+		kfifo->buf = kmalloc(cap);
+		if (!kfifo->buf) {
+			kfree(kfifo);
+			return NULL;
+		}
+	}
+	kfifo->siz = 0;
+	kfifo->cap = cap;
+	return kfifo;
+}
+
+void
+free_kfifo(struct kfifo *kfifo)
+{
+	spinlock_lock(&kfifo_lock);
+	kfree(kfifo->buf);
+	kfree(kfifo);
+	spinlock_unlock(&kfifo_lock);
+}
+
+int
+enqueue_kfifo(struct kfifo *kfifo, void *buf, size_t siz)
+{
+	void *tmp;
+
+	spinlock_lock(&kfifo_lock);
+	if (kfifo->cap - kfifo->siz >= siz) {
+		memcpy(kfifo->buf + kfifo->siz, buf, siz);
+		kfifo->siz += siz;
+		spinlock_unlock(&kfifo_lock);
+		return 0;
+	}
+
+	tmp = krealloc(kfifo->buf, kfifo->cap + siz);
+	if (!tmp) {
+		spinlock_unlock(&kfifo_lock);
+		return -1;
+	}
+	kfifo->buf = tmp;
+	memcpy(kfifo->buf + kfifo->siz, buf, siz);
+	kfifo->siz += siz;
+	kfifo->cap += siz;
+	spinlock_unlock(&kfifo_lock);
+	return 0;
+}
+
+int
+dequeue_kfifo(struct kfifo *kfifo, void *buf, size_t siz)
+{
+	spinlock_lock(&kfifo_lock);
+	if (kfifo->siz < siz) {
+		spinlock_unlock(&kfifo_lock);
+		return -1;
+	}
+	memcpy(buf, kfifo->buf, siz);
+	memmove(kfifo->buf, kfifo->buf + siz, kfifo->siz - siz);
+	kfifo->siz -= siz;
+	spinlock_unlock(&kfifo_lock);
+	return 0;
+}