diff --git a/src/main/common/streambuf.c b/src/main/common/streambuf.c new file mode 100644 index 0000000000..0f47511e03 --- /dev/null +++ b/src/main/common/streambuf.c @@ -0,0 +1,106 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include + +#include "streambuf.h" + +void sbufWriteU8(sbuf_t *dst, uint8_t val) +{ + *dst->ptr++ = val; +} + +void sbufWriteU16(sbuf_t *dst, uint16_t val) +{ + sbufWriteU8(dst, val >> 0); + sbufWriteU8(dst, val >> 8); +} + +void sbufWriteU32(sbuf_t *dst, uint32_t val) +{ + sbufWriteU8(dst, val >> 0); + sbufWriteU8(dst, val >> 8); + sbufWriteU8(dst, val >> 16); + sbufWriteU8(dst, val >> 24); +} + +void sbufWriteData(sbuf_t *dst, const void *data, int len) +{ + memcpy(dst->ptr, data, len); + dst->ptr += len; +} + +void sbufWriteString(sbuf_t *dst, const char *string) +{ + sbufWriteData(dst, string, strlen(string)); +} + +uint8_t sbufReadU8(sbuf_t *src) +{ + return *src->ptr++; +} + +uint16_t sbufReadU16(sbuf_t *src) +{ + uint16_t ret; + ret = sbufReadU8(src); + ret |= sbufReadU8(src) << 8; + return ret; +} + +uint32_t sbufReadU32(sbuf_t *src) +{ + uint32_t ret; + ret = sbufReadU8(src); + ret |= sbufReadU8(src) << 8; + ret |= sbufReadU8(src) << 16; + ret |= sbufReadU8(src) << 24; + return ret; +} + +void sbufReadData(sbuf_t *src, void *data, int len) +{ + memcpy(data, src->ptr, len); +} + +// reader - return bytes remaining in buffer +// writer - return available space +int sbufBytesRemaining(sbuf_t *buf) +{ + return buf->end - buf->ptr; +} + +uint8_t* sbufPtr(sbuf_t *buf) +{ + return buf->ptr; +} + +// advance buffer pointer +// reader - skip data +// writer - commit written data +void sbufAdvance(sbuf_t *buf, int size) +{ + buf->ptr += size; +} + +// modifies streambuf so that written data are prepared for reading +void sbufSwitchToReader(sbuf_t *buf, uint8_t *base) +{ + buf->end = buf->ptr; + buf->ptr = base; +} diff --git a/src/main/common/streambuf.h b/src/main/common/streambuf.h new file mode 100644 index 0000000000..7de771ef02 --- /dev/null +++ b/src/main/common/streambuf.h @@ -0,0 +1,45 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#include + +// simple buffer-based serializer/deserializer without implicit size check +// little-endian encoding implemneted now + +typedef struct sbuf_s { + uint8_t *ptr; // data pointer must be first (sbuff_t* is equivalent to uint8_t **) + uint8_t *end; +} sbuf_t; + +void sbufWriteU8(sbuf_t *dst, uint8_t val); +void sbufWriteU16(sbuf_t *dst, uint16_t val); +void sbufWriteU32(sbuf_t *dst, uint32_t val); +void sbufWriteData(sbuf_t *dst, const void *data, int len); +void sbufWriteString(sbuf_t *dst, const char *string); + +uint8_t sbufReadU8(sbuf_t *src); +uint16_t sbufReadU16(sbuf_t *src); +uint32_t sbufReadU32(sbuf_t *src); +void sbufReadData(sbuf_t *dst, void *data, int len); + +int sbufBytesRemaining(sbuf_t *buf); +uint8_t* sbufPtr(sbuf_t *buf); +void sbufAdvance(sbuf_t *buf, int size); + +void sbufSwitchToReader(sbuf_t *buf, uint8_t * base);