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 "platform.h"
  25
  26 #include "streambuf.h"
  27
  28 sbuf_t *sbufInit(sbuf_t *sbuf, uint8_t *ptr, uint8_t *end)
  29 {
  30     sbuf->ptr = ptr;
  31     sbuf->end = end;
  32     return sbuf;
  33 }
  34
  35 void sbufWriteU8(sbuf_t *dst, uint8_t val)
  36 {
  37     *dst->ptr++ = val;
  38 }
  39
  40 void sbufWriteU16(sbuf_t *dst, uint16_t val)
  41 {
  42     sbufWriteU8(dst, val >> 0);
  43     sbufWriteU8(dst, val >> 8);
  44 }
  45
  46 void sbufWriteU32(sbuf_t *dst, uint32_t val)
  47 {
  48     sbufWriteU8(dst, val >> 0);
  49     sbufWriteU8(dst, val >> 8);
  50     sbufWriteU8(dst, val >> 16);
  51     sbufWriteU8(dst, val >> 24);
  52 }
  53
  54 void sbufWriteU16BigEndian(sbuf_t *dst, uint16_t val)
  55 {
  56     sbufWriteU8(dst, val >> 8);
  57     sbufWriteU8(dst, (uint8_t)val);
  58 }
  59
  60 void sbufWriteU32BigEndian(sbuf_t *dst, uint32_t val)
  61 {
  62     sbufWriteU8(dst, val >> 24);
  63     sbufWriteU8(dst, val >> 16);
  64     sbufWriteU8(dst, val >> 8);
  65     sbufWriteU8(dst, (uint8_t)val);
  66 }
  67
  68
  69 void sbufFill(sbuf_t *dst, uint8_t data, int len)
  70 {
  71     memset(dst->ptr, data, len);
  72     dst->ptr += len;
  73 }
  74
  75 void sbufWriteData(sbuf_t *dst, const void *data, int len)
  76 {
  77     memcpy(dst->ptr, data, len);
  78     dst->ptr += len;
  79 }
  80
  81 void sbufWriteString(sbuf_t *dst, const char *string)
  82 {
  83     sbufWriteData(dst, string, strlen(string));
  84 }
  85
  86 void sbufWriteStringWithZeroTerminator(sbuf_t *dst, const char *string)
  87 {
  88     sbufWriteData(dst, string, strlen(string) + 1);
  89 }
  90
  91 uint8_t sbufReadU8(sbuf_t *src)
  92 {
  93     return *src->ptr++;
  94 }
  95
  96 uint16_t sbufReadU16(sbuf_t *src)
  97 {
  98     uint16_t ret;
  99     ret = sbufReadU8(src);
 100     ret |= sbufReadU8(src) << 8;
 101     return ret;
 102 }
 103
 104 uint32_t sbufReadU32(sbuf_t *src)
 105 {
 106     uint32_t ret;
 107     ret = sbufReadU8(src);
 108     ret |= sbufReadU8(src) <<  8;
 109     ret |= sbufReadU8(src) << 16;
 110     ret |= sbufReadU8(src) << 24;
 111     return ret;
 112 }
 113
 114 void sbufReadData(sbuf_t *src, void *data, int len)
 115 {
 116     memcpy(data, src->ptr, len);
 117 }
 118
 119 // reader - return bytes remaining in buffer
 120 // writer - return available space
 121 int sbufBytesRemaining(sbuf_t *buf)
 122 {
 123     return buf->end - buf->ptr;
 124 }
 125
 126 uint8_t* sbufPtr(sbuf_t *buf)
 127 {
 128     return buf->ptr;
 129 }
 130
 131 const uint8_t* sbufConstPtr(const sbuf_t *buf)
 132 {
 133     return buf->ptr;
 134 }
 135
 136 // advance buffer pointer
 137 // reader - skip data
 138 // writer - commit written data
 139 void sbufAdvance(sbuf_t *buf, int size)
 140 {
 141     buf->ptr += size;
 142 }
 143
 144 // modifies streambuf so that written data are prepared for reading
 145 void sbufSwitchToReader(sbuf_t *buf, uint8_t *base)
 146 {
 147     buf->end = buf->ptr;
 148     buf->ptr = base;
 149 }