flight/libraries/fifo_buffer.c

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       fifo_buffer.c
   5  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
   6  * @brief      GPIO input functions
   7  * @see        The GNU Public License (GPL) Version 3
   8  *
   9  *****************************************************************************/
  10 /*
  11  * This program is free software; you can redistribute it and/or modify
  12  * it under the terms of the GNU General Public License as published by
  13  * the Free Software Foundation; either version 3 of the License, or
  14  * (at your option) any later version.
  15  *
  16  * This program is distributed in the hope that it will be useful, but
  17  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  18  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  19  * for more details.
  20  *
  21  * You should have received a copy of the GNU General Public License along
  22  * with this program; if not, write to the Free Software Foundation, Inc.,
  23  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  24  */
  25
  26 #include <string.h>
  27
  28 #include "fifo_buffer.h"
  29
  30 // *****************************************************************************
  31 // circular buffer functions
  32
  33 uint16_t fifoBuf_getSize(t_fifo_buffer *buf)
  34 { // return the usable size of the buffer
  35     uint16_t buf_size = buf->buf_size;
  36
  37     if (buf_size > 0) {
  38         return buf_size - 1;
  39     } else {
  40         return 0;
  41     }
  42 }
  43
  44 uint16_t fifoBuf_getUsed(t_fifo_buffer *buf)
  45 { // return the number of bytes available in the rx buffer
  46     uint16_t rd = buf->rd;
  47     uint16_t wr = buf->wr;
  48     uint16_t buf_size  = buf->buf_size;
  49
  50     uint16_t num_bytes = wr - rd;
  51
  52     if (wr < rd) {
  53         num_bytes = (buf_size - rd) + wr;
  54     }
  55
  56     return num_bytes;
  57 }
  58
  59 uint16_t fifoBuf_getFree(t_fifo_buffer *buf)
  60 { // return the free space size in the buffer
  61     uint16_t buf_size  = buf->buf_size;
  62
  63     uint16_t num_bytes = fifoBuf_getUsed(buf);
  64
  65     return (buf_size - num_bytes) - 1;
  66 }
  67
  68 void fifoBuf_clearData(t_fifo_buffer *buf)
  69 { // remove all data from the buffer
  70     buf->rd = buf->wr;
  71 }
  72
  73 void fifoBuf_removeData(t_fifo_buffer *buf, uint16_t len)
  74 { // remove a number of bytes from the buffer
  75     uint16_t rd = buf->rd;
  76     uint16_t buf_size  = buf->buf_size;
  77
  78     // get number of bytes available
  79     uint16_t num_bytes = fifoBuf_getUsed(buf);
  80
  81     if (num_bytes > len) {
  82         num_bytes = len;
  83     }
  84
  85     if (num_bytes < 1) {
  86         return; // nothing to remove
  87     }
  88     rd += num_bytes;
  89     if (rd >= buf_size) {
  90         rd -= buf_size;
  91     }
  92
  93     buf->rd = rd;
  94 }
  95
  96 int16_t fifoBuf_getBytePeek(t_fifo_buffer *buf)
  97 { // get a data byte from the buffer without removing it
  98     uint16_t rd = buf->rd;
  99
 100     // get number of bytes available
 101     uint16_t num_bytes = fifoBuf_getUsed(buf);
 102
 103     if (num_bytes < 1) {
 104         return -1; // no byte retuened
 105     }
 106     return buf->buf_ptr[rd]; // return the byte
 107 }
 108
 109 int16_t fifoBuf_getByte(t_fifo_buffer *buf)
 110 { // get a data byte from the buffer
 111     uint16_t rd        = buf->rd;
 112     uint16_t buf_size  = buf->buf_size;
 113     uint8_t *buff      = buf->buf_ptr;
 114
 115     // get number of bytes available
 116     uint16_t num_bytes = fifoBuf_getUsed(buf);
 117
 118     if (num_bytes < 1) {
 119         return -1; // no byte returned
 120     }
 121     uint8_t b = buff[rd];
 122     if (++rd >= buf_size) {
 123         rd = 0;
 124     }
 125
 126     buf->rd = rd;
 127
 128     return b; // return the byte
 129 }
 130
 131 uint16_t fifoBuf_getDataPeek(t_fifo_buffer *buf, void *data, uint16_t len)
 132 { // get data from the buffer without removing it
 133     uint16_t rd        = buf->rd;
 134     uint16_t buf_size  = buf->buf_size;
 135     uint8_t *buff      = buf->buf_ptr;
 136
 137     // get number of bytes available
 138     uint16_t num_bytes = fifoBuf_getUsed(buf);
 139
 140     if (num_bytes > len) {
 141         num_bytes = len;
 142     }
 143
 144     if (num_bytes < 1) {
 145         return 0; // return number of bytes copied
 146     }
 147     uint8_t *p = (uint8_t *)data;
 148     uint16_t i = 0;
 149
 150     while (num_bytes > 0) {
 151         uint16_t block_len = buf_size - rd;
 152         if (block_len > num_bytes) {
 153             block_len = num_bytes;
 154         }
 155         if (block_len == 1) {
 156             *((uint8_t *)(p + i)) = *((uint8_t *)(buff + rd));
 157         } else {
 158             memcpy(p + i, buff + rd, block_len);
 159         }
 160         i  += block_len;
 161         num_bytes -= block_len;
 162         rd += block_len;
 163         if (rd >= buf_size) {
 164             rd = 0;
 165         }
 166     }
 167
 168     return i; // return number of bytes copied
 169 }
 170
 171 uint16_t fifoBuf_getData(t_fifo_buffer *buf, void *data, uint16_t len)
 172 { // get data from our rx buffer
 173     uint16_t rd        = buf->rd;
 174     uint16_t buf_size  = buf->buf_size;
 175     uint8_t *buff      = buf->buf_ptr;
 176
 177     // get number of bytes available
 178     uint16_t num_bytes = fifoBuf_getUsed(buf);
 179
 180     if (num_bytes > len) {
 181         num_bytes = len;
 182     }
 183
 184     if (num_bytes < 1) {
 185         return 0; // return number of bytes copied
 186     }
 187     uint8_t *p = (uint8_t *)data;
 188     uint16_t i = 0;
 189
 190     while (num_bytes > 0) {
 191         uint16_t block_len = buf_size - rd;
 192         if (block_len > num_bytes) {
 193             block_len = num_bytes;
 194         }
 195         if (block_len == 1) {
 196             *((uint8_t *)(p + i)) = *((uint8_t *)(buff + rd));
 197         } else {
 198             memcpy(p + i, buff + rd, block_len);
 199         }
 200         i  += block_len;
 201         num_bytes -= block_len;
 202         rd += block_len;
 203         if (rd >= buf_size) {
 204             rd = 0;
 205         }
 206     }
 207
 208     buf->rd = rd;
 209
 210     return i; // return number of bytes copied
 211 }
 212
 213 uint16_t fifoBuf_putByte(t_fifo_buffer *buf, const uint8_t b)
 214 { // add a data byte to the buffer
 215     uint16_t wr        = buf->wr;
 216     uint16_t buf_size  = buf->buf_size;
 217     uint8_t *buff      = buf->buf_ptr;
 218
 219     uint16_t num_bytes = fifoBuf_getFree(buf);
 220
 221     if (num_bytes < 1) {
 222         return 0;
 223     }
 224
 225     buff[wr] = b;
 226     if (++wr >= buf_size) {
 227         wr = 0;
 228     }
 229
 230     buf->wr = wr;
 231
 232     return 1; // return number of bytes copied
 233 }
 234
 235 uint16_t fifoBuf_putData(t_fifo_buffer *buf, const void *data, uint16_t len)
 236 { // add data to the buffer
 237     uint16_t wr        = buf->wr;
 238     uint16_t buf_size  = buf->buf_size;
 239     uint8_t *buff      = buf->buf_ptr;
 240
 241     uint16_t num_bytes = fifoBuf_getFree(buf);
 242
 243     if (num_bytes > len) {
 244         num_bytes = len;
 245     }
 246
 247     if (num_bytes < 1) {
 248         return 0; // return number of bytes copied
 249     }
 250     uint8_t *p = (uint8_t *)data;
 251     uint16_t i = 0;
 252
 253     while (num_bytes > 0) {
 254         uint16_t block_len = buf_size - wr;
 255         if (block_len > num_bytes) {
 256             block_len = num_bytes;
 257         }
 258         if (block_len == 1) {
 259             *((uint8_t *)(buff + wr)) = *((uint8_t *)(p + i));
 260         } else {
 261             memcpy(buff + wr, p + i, block_len);
 262         }
 263         i  += block_len;
 264         num_bytes -= block_len;
 265         wr += block_len;
 266         if (wr >= buf_size) {
 267             wr = 0;
 268         }
 269     }
 270
 271     buf->wr = wr;
 272
 273     return i; // return number of bytes copied
 274 }
 275
 276 void fifoBuf_init(t_fifo_buffer *buf, const void *buffer, const uint16_t buffer_size)
 277 {
 278     buf->buf_ptr  = (uint8_t *)buffer;
 279     buf->rd = 0;
 280     buf->wr = 0;
 281     buf->buf_size = buffer_size;
 282 }
 283
 284 // *****************************************************************************