lunaix-os/kernel/ds/fifo.c

   1 #include <klibc/string.h>
   2 #include <lunaix/ds/fifo.h>
   3 #include <lunaix/ds/mutex.h>
   4 #include <lunaix/spike.h>
   5
   6 void
   7 fifo_init(struct fifo_buf* buf, void* data_buffer, size_t buf_size, int flags)
   8 {
   9     *buf = (struct fifo_buf){ .data = data_buffer,
  10                               .rd_pos = 0,
  11                               .wr_pos = 0,
  12                               .size = buf_size,
  13                               .flags = flags,
  14                               .free_len = buf_size };
  15     mutex_init(&buf->lock);
  16 }
  17
  18 int
  19 fifo_backone(struct fifo_buf* fbuf)
  20 {
  21     mutex_lock(&fbuf->lock);
  22
  23     if (fbuf->free_len == fbuf->size) {
  24         mutex_unlock(&fbuf->lock);
  25         return 0;
  26     }
  27
  28     fbuf->wr_pos = (fbuf->wr_pos ? fbuf->wr_pos : fbuf->size) - 1;
  29     fbuf->free_len++;
  30
  31     mutex_unlock(&fbuf->lock);
  32
  33     return 1;
  34 }
  35
  36 size_t
  37 fifo_putone(struct fifo_buf* fbuf, uint8_t data)
  38 {
  39     mutex_lock(&fbuf->lock);
  40
  41     if (!fbuf->free_len) {
  42         mutex_unlock(&fbuf->lock);
  43         return 0;
  44     }
  45
  46     uint8_t* dest = fbuf->data;
  47     dest[fbuf->wr_pos] = data;
  48     fbuf->wr_pos = (fbuf->wr_pos + 1) % fbuf->size;
  49     fbuf->free_len--;
  50
  51     mutex_unlock(&fbuf->lock);
  52
  53     return 1;
  54 }
  55
  56 size_t
  57 fifo_write(struct fifo_buf* fbuf, void* data, size_t count)
  58 {
  59     size_t wr_count = 0, wr_pos = fbuf->wr_pos;
  60
  61     mutex_lock(&fbuf->lock);
  62
  63     if (!fbuf->free_len) {
  64         mutex_unlock(&fbuf->lock);
  65         return 0;
  66     }
  67
  68     if (wr_pos >= fbuf->rd_pos) {
  69         // case 1
  70         size_t cplen_tail = MIN(fbuf->size - wr_pos, count);
  71         size_t cplen_head = MIN(fbuf->rd_pos, count - cplen_tail);
  72         memcpy(fbuf->data + wr_pos, data, cplen_tail);
  73         memcpy(fbuf->data, data + cplen_tail, cplen_head);
  74
  75         wr_count = cplen_head + cplen_tail;
  76     } else {
  77         // case 2
  78         wr_count = MIN(fbuf->rd_pos - wr_pos, count);
  79         memcpy(fbuf->data + wr_pos, data, wr_count);
  80     }
  81
  82     fbuf->wr_pos = (wr_pos + wr_count) % fbuf->size;
  83     fbuf->free_len -= wr_count;
  84
  85     mutex_unlock(&fbuf->lock);
  86
  87     return wr_count;
  88 }
  89
  90 size_t
  91 fifo_read(struct fifo_buf* fbuf, void* buf, size_t count)
  92 {
  93     size_t rd_count = 0, rd_pos = fbuf->rd_pos;
  94     mutex_lock(&fbuf->lock);
  95
  96     if (fbuf->free_len == fbuf->size) {
  97         mutex_unlock(&fbuf->lock);
  98         return 0;
  99     }
 100
 101     if (rd_pos >= fbuf->wr_pos) {
 102         size_t cplen_tail = MIN(fbuf->size - rd_pos, count);
 103         size_t cplen_head = MIN(fbuf->wr_pos, count - cplen_tail);
 104         memcpy(buf, fbuf->data + rd_pos, cplen_tail);
 105         memcpy(buf + cplen_tail, fbuf->data, cplen_head);
 106
 107         rd_count = cplen_head + cplen_tail;
 108     } else {
 109         rd_count = MIN(fbuf->wr_pos - rd_pos, count);
 110         memcpy(buf, fbuf->data + rd_pos, rd_count);
 111     }
 112
 113     fbuf->rd_pos = (rd_pos + rd_count) % fbuf->size;
 114     fbuf->free_len += rd_count;
 115
 116     mutex_unlock(&fbuf->lock);
 117
 118     return rd_count;
 119 }