voron

experimental ARM OS
git clone git@git.2f30.org/voron.git
Log | Files | Refs | README | LICENSE

kfifo.c (1456B)


      1 #include <kernel.h>
      2 #include <alloc.h>
      3 #include <spinlock.h>
      4 #include <kfifo.h>
      5 
      6 struct kfifo {
      7 	uint8_t *buf;
      8 	size_t siz;
      9 	size_t cap;
     10 	spinlock_t lock;
     11 };
     12 
     13 struct kfifo *
     14 init_kfifo(size_t cap)
     15 {
     16 	struct kfifo *kfifo;
     17 
     18 	kfifo = kmalloc(sizeof(*kfifo));
     19 	if (!kfifo)
     20 		return NULL;
     21 	if (cap > 0) {
     22 		kfifo->buf = kmalloc(cap);
     23 		if (!kfifo->buf) {
     24 			kfree(kfifo);
     25 			return NULL;
     26 		}
     27 	}
     28 	kfifo->siz = 0;
     29 	kfifo->cap = cap;
     30 	spinlock_init(&kfifo->lock);
     31 	return kfifo;
     32 }
     33 
     34 void
     35 free_kfifo(struct kfifo *kfifo)
     36 {
     37 	kfree(kfifo->buf);
     38 	kfree(kfifo);
     39 }
     40 
     41 int
     42 enqueue_kfifo(struct kfifo *kfifo, const void *buf, size_t siz)
     43 {
     44 	void *tmp;
     45 
     46 	if (!siz)
     47 		return 0;
     48 
     49 	spinlock_lock(&kfifo->lock);
     50 	if (kfifo->cap - kfifo->siz >= siz) {
     51 		memcpy(kfifo->buf + kfifo->siz, buf, siz);
     52 		kfifo->siz += siz;
     53 		spinlock_unlock(&kfifo->lock);
     54 		return 0;
     55 	}
     56 
     57 	tmp = krealloc(kfifo->buf, kfifo->cap + siz);
     58 	if (!tmp) {
     59 		spinlock_unlock(&kfifo->lock);
     60 		return -ENOMEM;
     61 	}
     62 	kfifo->buf = tmp;
     63 	memcpy(kfifo->buf + kfifo->siz, buf, siz);
     64 	kfifo->siz += siz;
     65 	kfifo->cap += siz;
     66 	spinlock_unlock(&kfifo->lock);
     67 	return 0;
     68 }
     69 
     70 int
     71 dequeue_kfifo(struct kfifo *kfifo, void *buf, size_t siz)
     72 {
     73 	if (!siz)
     74 		return 0;
     75 
     76 	spinlock_lock(&kfifo->lock);
     77 	if (kfifo->siz < siz) {
     78 		spinlock_unlock(&kfifo->lock);
     79 		return -EINVAL;
     80 	}
     81 	memcpy(buf, kfifo->buf, siz);
     82 	memmove(kfifo->buf, kfifo->buf + siz, kfifo->siz - siz);
     83 	kfifo->siz -= siz;
     84 	spinlock_unlock(&kfifo->lock);
     85 	return 0;
     86 }