src/main/common/streambuf.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <string.h>
  22 #include <stdint.h>
  23
  24 #include "streambuf.h"
  25
  26 sbuf_t *sbufInit(sbuf_t *sbuf, uint8_t *ptr, uint8_t *end)
  27 {
  28     sbuf->ptr = ptr;
  29     sbuf->end = end;
  30     return sbuf;
  31 }
  32
  33 void sbufWriteU8(sbuf_t *dst, uint8_t val)
  34 {
  35     *dst->ptr++ = val;
  36 }
  37
  38 void sbufWriteU16(sbuf_t *dst, uint16_t val)
  39 {
  40     sbufWriteU8(dst, val >> 0);
  41     sbufWriteU8(dst, val >> 8);
  42 }
  43
  44 void sbufWriteU32(sbuf_t *dst, uint32_t val)
  45 {
  46     sbufWriteU8(dst, val >> 0);
  47     sbufWriteU8(dst, val >> 8);
  48     sbufWriteU8(dst, val >> 16);
  49     sbufWriteU8(dst, val >> 24);
  50 }
  51
  52 void sbufWriteU16BigEndian(sbuf_t *dst, uint16_t val)
  53 {
  54     sbufWriteU8(dst, val >> 8);
  55     sbufWriteU8(dst, (uint8_t)val);
  56 }
  57
  58 void sbufWriteU32BigEndian(sbuf_t *dst, uint32_t val)
  59 {
  60     sbufWriteU8(dst, val >> 24);
  61     sbufWriteU8(dst, val >> 16);
  62     sbufWriteU8(dst, val >> 8);
  63     sbufWriteU8(dst, (uint8_t)val);
  64 }
  65
  66
  67 void sbufFill(sbuf_t *dst, uint8_t data, int len)
  68 {
  69     memset(dst->ptr, data, len);
  70     dst->ptr += len;
  71 }
  72
  73 void sbufWriteData(sbuf_t *dst, const void *data, int len)
  74 {
  75     memcpy(dst->ptr, data, len);
  76     dst->ptr += len;
  77 }
  78
  79 void sbufWriteString(sbuf_t *dst, const char *string)
  80 {
  81     sbufWriteData(dst, string, strlen(string));
  82 }
  83
  84 void sbufWriteStringWithZeroTerminator(sbuf_t *dst, const char *string)
  85 {
  86     sbufWriteData(dst, string, strlen(string) + 1);
  87 }
  88
  89 uint8_t sbufReadU8(sbuf_t *src)
  90 {
  91     return *src->ptr++;
  92 }
  93
  94 uint16_t sbufReadU16(sbuf_t *src)
  95 {
  96     uint16_t ret;
  97     ret = sbufReadU8(src);
  98     ret |= sbufReadU8(src) << 8;
  99     return ret;
 100 }
 101
 102 uint32_t sbufReadU32(sbuf_t *src)
 103 {
 104     uint32_t ret;
 105     ret = sbufReadU8(src);
 106     ret |= sbufReadU8(src) <<  8;
 107     ret |= sbufReadU8(src) << 16;
 108     ret |= sbufReadU8(src) << 24;
 109     return ret;
 110 }
 111
 112 void sbufReadData(sbuf_t *src, void *data, int len)
 113 {
 114     memcpy(data, src->ptr, len);
 115 }
 116
 117 // reader - return bytes remaining in buffer
 118 // writer - return available space
 119 int sbufBytesRemaining(sbuf_t *buf)
 120 {
 121     return buf->end - buf->ptr;
 122 }
 123
 124 uint8_t* sbufPtr(sbuf_t *buf)
 125 {
 126     return buf->ptr;
 127 }
 128
 129 const uint8_t* sbufConstPtr(const sbuf_t *buf)
 130 {
 131     return buf->ptr;
 132 }
 133
 134 // advance buffer pointer
 135 // reader - skip data
 136 // writer - commit written data
 137 void sbufAdvance(sbuf_t *buf, int size)
 138 {
 139     buf->ptr += size;
 140 }
 141
 142 // modifies streambuf so that written data are prepared for reading
 143 void sbufSwitchToReader(sbuf_t *buf, uint8_t *base)
 144 {
 145     buf->end = buf->ptr;
 146     buf->ptr = base;
 147 }