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_readone_async(struct fifo_buf* fbuf, uint8_t* data)
  58 {
  59     if (fbuf->free_len == fbuf->size) {
  60         return 0;
  61     }
  62
  63     uint8_t* dest = fbuf->data;
  64     *data = dest[fbuf->rd_pos];
  65     fbuf->rd_pos = (fbuf->rd_pos + 1) % fbuf->size;
  66     fbuf->free_len++;
  67
  68     return 1;
  69 }
  70
  71 void
  72 fifo_set_rdptr(struct fifo_buf* fbuf, size_t rdptr)
  73 {
  74     fbuf->rd_pos = rdptr;
  75     if (rdptr <= fbuf->wr_pos) {
  76         fbuf->free_len = fbuf->size - fbuf->wr_pos + rdptr;
  77     } else {
  78         fbuf->free_len = rdptr - fbuf->wr_pos;
  79     }
  80 }
  81
  82 void
  83 fifo_set_wrptr(struct fifo_buf* fbuf, size_t wrptr)
  84 {
  85     fbuf->wr_pos = wrptr;
  86     if (wrptr <= fbuf->rd_pos) {
  87         fbuf->free_len = fbuf->size - fbuf->rd_pos + wrptr;
  88     } else {
  89         fbuf->free_len = wrptr - fbuf->rd_pos;
  90     }
  91 }
  92
  93 size_t
  94 fifo_write(struct fifo_buf* fbuf, void* data, size_t count)
  95 {
  96     size_t wr_count = 0, wr_pos = fbuf->wr_pos;
  97
  98     mutex_lock(&fbuf->lock);
  99
 100     if (!fbuf->free_len) {
 101         mutex_unlock(&fbuf->lock);
 102         return 0;
 103     }
 104
 105     if (wr_pos >= fbuf->rd_pos) {
 106         // case 1
 107         size_t cplen_tail = MIN(fbuf->size - wr_pos, count);
 108         size_t cplen_head = MIN(fbuf->rd_pos, count - cplen_tail);
 109         memcpy(fbuf->data + wr_pos, data, cplen_tail);
 110         memcpy(fbuf->data, data + cplen_tail, cplen_head);
 111
 112         wr_count = cplen_head + cplen_tail;
 113     } else {
 114         // case 2
 115         wr_count = MIN(fbuf->rd_pos - wr_pos, count);
 116         memcpy(fbuf->data + wr_pos, data, wr_count);
 117     }
 118
 119     fbuf->wr_pos = (wr_pos + wr_count) % fbuf->size;
 120     fbuf->free_len -= wr_count;
 121
 122     mutex_unlock(&fbuf->lock);
 123
 124     return wr_count;
 125 }
 126
 127 size_t
 128 fifo_read(struct fifo_buf* fbuf, void* buf, size_t count)
 129 {
 130     size_t rd_count = 0, rd_pos = fbuf->rd_pos;
 131     mutex_lock(&fbuf->lock);
 132
 133     if (fbuf->free_len == fbuf->size) {
 134         mutex_unlock(&fbuf->lock);
 135         return 0;
 136     }
 137
 138     if (rd_pos >= fbuf->wr_pos) {
 139         size_t cplen_tail = MIN(fbuf->size - rd_pos, count);
 140         size_t cplen_head = MIN(fbuf->wr_pos, count - cplen_tail);
 141         memcpy(buf, fbuf->data + rd_pos, cplen_tail);
 142         memcpy(buf + cplen_tail, fbuf->data, cplen_head);
 143
 144         rd_count = cplen_head + cplen_tail;
 145     } else {
 146         rd_count = MIN(fbuf->wr_pos - rd_pos, count);
 147         memcpy(buf, fbuf->data + rd_pos, rd_count);
 148     }
 149
 150     fbuf->rd_pos = (rd_pos + rd_count) % fbuf->size;
 151     fbuf->free_len += rd_count;
 152
 153     mutex_unlock(&fbuf->lock);
 154
 155     return rd_count;
 156 }