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 void
  19 fifo_clear(struct fifo_buf* fbuf)
  20 {
  21     mutex_lock(&fbuf->lock);
  22     fbuf->rd_pos = 0;
  23     fbuf->wr_pos = 0;
  24     fbuf->free_len = fbuf->size;
  25     mutex_unlock(&fbuf->lock);
  26 }
  27
  28 int
  29 fifo_backone(struct fifo_buf* fbuf)
  30 {
  31     mutex_lock(&fbuf->lock);
  32
  33     if (fbuf->free_len == fbuf->size) {
  34         mutex_unlock(&fbuf->lock);
  35         return 0;
  36     }
  37
  38     fbuf->wr_pos = (fbuf->wr_pos ? fbuf->wr_pos : fbuf->size) - 1;
  39     fbuf->free_len++;
  40
  41     mutex_unlock(&fbuf->lock);
  42
  43     return 1;
  44 }
  45
  46 size_t
  47 fifo_putone(struct fifo_buf* fbuf, u8_t data)
  48 {
  49     mutex_lock(&fbuf->lock);
  50
  51     if (!fbuf->free_len) {
  52         mutex_unlock(&fbuf->lock);
  53         return 0;
  54     }
  55
  56     u8_t* dest = fbuf->data;
  57     dest[fbuf->wr_pos] = data;
  58     fbuf->wr_pos = (fbuf->wr_pos + 1) % fbuf->size;
  59     fbuf->free_len--;
  60
  61     mutex_unlock(&fbuf->lock);
  62
  63     return 1;
  64 }
  65
  66 size_t
  67 fifo_readone_async(struct fifo_buf* fbuf, u8_t* data)
  68 {
  69     if (fbuf->free_len == fbuf->size) {
  70         return 0;
  71     }
  72
  73     u8_t* dest = fbuf->data;
  74     *data = dest[fbuf->rd_pos];
  75     fbuf->rd_pos = (fbuf->rd_pos + 1) % fbuf->size;
  76     fbuf->free_len++;
  77
  78     return 1;
  79 }
  80
  81 size_t
  82 fifo_readone(struct fifo_buf* fbuf, u8_t* data)
  83 {
  84     mutex_lock(&fbuf->lock);
  85     size_t retval = fifo_readone_async(fbuf, data);
  86     mutex_unlock(&fbuf->lock);
  87
  88     return retval;
  89 }
  90
  91 void
  92 fifo_set_rdptr(struct fifo_buf* fbuf, size_t rdptr)
  93 {
  94     fbuf->rd_pos = rdptr;
  95     if (rdptr <= fbuf->wr_pos) {
  96         fbuf->free_len = fbuf->size - fbuf->wr_pos + rdptr;
  97     } else {
  98         fbuf->free_len = rdptr - fbuf->wr_pos;
  99     }
 100 }
 101
 102 void
 103 fifo_set_wrptr(struct fifo_buf* fbuf, size_t wrptr)
 104 {
 105     fbuf->wr_pos = wrptr;
 106     if (wrptr <= fbuf->rd_pos) {
 107         fbuf->free_len = fbuf->size - fbuf->rd_pos + wrptr;
 108     } else {
 109         fbuf->free_len = wrptr - fbuf->rd_pos;
 110     }
 111 }
 112
 113 size_t
 114 fifo_write(struct fifo_buf* fbuf, void* data, size_t count)
 115 {
 116     size_t wr_count = 0, wr_pos = fbuf->wr_pos;
 117
 118     if (!count) {
 119         return 0;
 120     }
 121
 122     mutex_lock(&fbuf->lock);
 123
 124     if (!fbuf->free_len) {
 125         mutex_unlock(&fbuf->lock);
 126         return 0;
 127     }
 128
 129     if (wr_pos >= fbuf->rd_pos) {
 130         // case 1
 131         size_t cplen_tail = MIN(fbuf->size - wr_pos, count);
 132         size_t cplen_head = MIN(fbuf->rd_pos, count - cplen_tail);
 133         memcpy(fbuf->data + wr_pos, data, cplen_tail);
 134         memcpy(fbuf->data, data + cplen_tail, cplen_head);
 135
 136         wr_count = cplen_head + cplen_tail;
 137     } else {
 138         // case 2
 139         wr_count = MIN(fbuf->rd_pos - wr_pos, count);
 140         memcpy(fbuf->data + wr_pos, data, wr_count);
 141     }
 142
 143     fbuf->wr_pos = (wr_pos + wr_count) % fbuf->size;
 144     fbuf->free_len -= wr_count;
 145
 146     mutex_unlock(&fbuf->lock);
 147
 148     return wr_count;
 149 }
 150
 151 size_t
 152 fifo_read(struct fifo_buf* fbuf, void* buf, size_t count)
 153 {
 154     size_t rd_count = 0, rd_pos = fbuf->rd_pos;
 155     mutex_lock(&fbuf->lock);
 156
 157     if (fbuf->free_len == fbuf->size) {
 158         mutex_unlock(&fbuf->lock);
 159         return 0;
 160     }
 161
 162     if (rd_pos >= fbuf->wr_pos) {
 163         size_t cplen_tail = MIN(fbuf->size - rd_pos, count);
 164         size_t cplen_head = MIN(fbuf->wr_pos, count - cplen_tail);
 165         memcpy(buf, fbuf->data + rd_pos, cplen_tail);
 166         memcpy(buf + cplen_tail, fbuf->data, cplen_head);
 167
 168         rd_count = cplen_head + cplen_tail;
 169     } else {
 170         rd_count = MIN(fbuf->wr_pos - rd_pos, count);
 171         memcpy(buf, fbuf->data + rd_pos, rd_count);
 172     }
 173
 174     fbuf->rd_pos = (rd_pos + rd_count) % fbuf->size;
 175     fbuf->free_len += rd_count;
 176
 177     mutex_unlock(&fbuf->lock);
 178
 179     return rd_count;
 180 }