src/main/common/streambuf.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <string.h>
  19 #include <stdint.h>
  20
  21 #include "streambuf.h"
  22
  23 sbuf_t *sbufInit(sbuf_t *sbuf, uint8_t *ptr, uint8_t *end)
  24 {
  25     sbuf->ptr = ptr;
  26     sbuf->end = end;
  27     return sbuf;
  28 }
  29
  30 void sbufWriteU8(sbuf_t *dst, uint8_t val)
  31 {
  32     *dst->ptr++ = val;
  33 }
  34
  35 void sbufWriteU16(sbuf_t *dst, uint16_t val)
  36 {
  37     sbufWriteU8(dst, val >> 0);
  38     sbufWriteU8(dst, val >> 8);
  39 }
  40
  41 void sbufWriteU32(sbuf_t *dst, uint32_t val)
  42 {
  43     sbufWriteU8(dst, val >> 0);
  44     sbufWriteU8(dst, val >> 8);
  45     sbufWriteU8(dst, val >> 16);
  46     sbufWriteU8(dst, val >> 24);
  47 }
  48
  49 void sbufWriteU16BigEndian(sbuf_t *dst, uint16_t val)
  50 {
  51     sbufWriteU8(dst, val >> 8);
  52     sbufWriteU8(dst, val >> 0);
  53 }
  54
  55 void sbufWriteU32BigEndian(sbuf_t *dst, uint32_t val)
  56 {
  57     sbufWriteU8(dst, val >> 24);
  58     sbufWriteU8(dst, val >> 16);
  59     sbufWriteU8(dst, val >> 8);
  60     sbufWriteU8(dst, val >> 0);
  61 }
  62
  63 void sbufFill(sbuf_t *dst, uint8_t data, int len)
  64 {
  65     memset(dst->ptr, data, len);
  66     dst->ptr += len;
  67 }
  68
  69 void sbufWriteData(sbuf_t *dst, const void *data, int len)
  70 {
  71     memcpy(dst->ptr, data, len);
  72     dst->ptr += len;
  73 }
  74
  75 bool sbufWriteDataSafe(sbuf_t *dst, const void *data, int len)
  76 {
  77     // only write if data does not overflow buffer
  78     if (sbufBytesRemaining(dst) >= len) {
  79         memcpy(dst->ptr, data, len);
  80         dst->ptr += len;
  81         return true;
  82     }
  83     return false;
  84 }
  85
  86 void sbufWriteString(sbuf_t *dst, const char *string)
  87 {
  88     sbufWriteData(dst, string, strlen(string));
  89 }
  90
  91 void sbufWriteStringWithZeroTerminator(sbuf_t *dst, const char *string)
  92 {
  93     sbufWriteData(dst, string, strlen(string) + 1);
  94 }
  95
  96 uint8_t sbufReadU8(sbuf_t *src)
  97 {
  98     return *src->ptr++;
  99 }
 100
 101 uint16_t sbufReadU16(sbuf_t *src)
 102 {
 103     uint16_t ret;
 104     ret = sbufReadU8(src);
 105     ret |= sbufReadU8(src) << 8;
 106     return ret;
 107 }
 108
 109 uint32_t sbufReadU32(sbuf_t *src)
 110 {
 111     uint32_t ret;
 112     ret = sbufReadU8(src);
 113     ret |= sbufReadU8(src) <<  8;
 114     ret |= sbufReadU8(src) << 16;
 115     ret |= sbufReadU8(src) << 24;
 116     return ret;
 117 }
 118
 119 void sbufReadData(const sbuf_t *src, void *data, int len)
 120 {
 121     memcpy(data, src->ptr, len);
 122 }
 123
 124 bool sbufReadU8Safe(uint8_t *dst, sbuf_t *src)
 125 {
 126     if (sbufBytesRemaining(src)) {
 127         const uint8_t value = sbufReadU8(src);
 128         if (dst) {
 129             *dst = value;
 130         }
 131         return true;
 132     }
 133     return false;
 134 }
 135
 136 bool sbufReadI8Safe(int8_t *dst, sbuf_t *src)
 137 {
 138     return sbufReadU8Safe((uint8_t*)dst, src);
 139 }
 140
 141 bool sbufReadU16Safe(uint16_t *dst, sbuf_t *src)
 142 {
 143     if (sbufBytesRemaining(src) >= 2) { // check there are enough bytes left in the buffer to read a uint16_t
 144         const uint16_t value = sbufReadU16(src);
 145         if (dst) {
 146             *dst = value;
 147         }
 148         return true;
 149     }
 150     return false;
 151 }
 152
 153 bool sbufReadI16Safe(int16_t *dst, sbuf_t *src)
 154 {
 155     return sbufReadU16Safe((uint16_t*)dst, src);
 156 }
 157
 158 bool sbufReadU32Safe(uint32_t *dst, sbuf_t *src)
 159 {
 160     if (sbufBytesRemaining(src) >= 4) { // check there are enough bytes left in the buffer to read a uint32_t
 161         const uint32_t value = sbufReadU32(src);
 162         if (dst) {
 163             *dst = value;
 164         }
 165         return true;
 166     }
 167     return false;
 168 }
 169
 170 bool sbufReadI32Safe(int32_t *dst, sbuf_t *src)
 171 {
 172     return sbufReadU32Safe((uint32_t*)dst, src);
 173 }
 174
 175 bool sbufReadDataSafe(const sbuf_t *src, void *data, int len)
 176 {
 177     if (sbufBytesRemaining(src) >= len) {
 178         sbufReadData(src, data, len);
 179         return true;
 180     }
 181     return false;
 182 }
 183
 184 // reader - return bytes remaining in buffer
 185 // writer - return available space
 186 int sbufBytesRemaining(const sbuf_t *buf)
 187 {
 188     return buf->end - buf->ptr;
 189 }
 190
 191 uint8_t* sbufPtr(sbuf_t *buf)
 192 {
 193     return buf->ptr;
 194 }
 195
 196 const uint8_t* sbufConstPtr(const sbuf_t *buf)
 197 {
 198     return buf->ptr;
 199 }
 200
 201 // advance buffer pointer
 202 // reader - skip data
 203 // writer - commit written data
 204 void sbufAdvance(sbuf_t *buf, int size)
 205 {
 206     buf->ptr += size;
 207 }
 208
 209 // modifies streambuf so that written data are prepared for reading
 210 void sbufSwitchToReader(sbuf_t *buf, uint8_t *base)
 211 {
 212     buf->end = buf->ptr;
 213     buf->ptr = base;
 214 }