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