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 }