Skip to content

Commit

Permalink
Refactor streambuf
Browse files Browse the repository at this point in the history
  • Loading branch information
@pmattila
pmattila committed Jul 15, 2024
1 parent 7af085a commit a535da9
Show file tree
Hide file tree
Showing 3 changed files with 201 additions and 77 deletions.
179 changes: 121 additions & 58 deletions src/main/common/streambuf.c
View file
Original file line number Diff line number Diff line change
@@ -1,28 +1,27 @@
/*
* This file is part of Cleanflight and Betaflight.
* This file is part of Rotorflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software 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.
* Rotorflight 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 and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* Rotorflight 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 this software.
*
* If not, see <http://www.gnu.org/licenses/>.
* along with this software. If not, see <https://www.gnu.org/licenses/>.
*/

#include <string.h>
#include <stdint.h>

#include "platform.h"

#include "common/utils.h"

#include "streambuf.h"

sbuf_t *sbufInit(sbuf_t *sbuf, uint8_t *ptr, uint8_t *end)
Expand All @@ -32,43 +31,83 @@ sbuf_t *sbufInit(sbuf_t *sbuf, uint8_t *ptr, uint8_t *end)
return sbuf;
}

#define SBUFPUSH(dst, val) ((*(dst)->ptr++ = (val)))
#define SBUFPOP(src) ((*(src)->ptr++))

void sbufWriteU8(sbuf_t *dst, uint8_t val)
{
*dst->ptr++ = val;
SBUFPUSH(dst,val);
}

void sbufWriteU16(sbuf_t *dst, uint16_t val)
{
sbufWriteU8(dst, val >> 0);
sbufWriteU8(dst, val >> 8);
SBUFPUSH(dst, val >> 0);
SBUFPUSH(dst, val >> 8);
}

void sbufWriteU16BE(sbuf_t *dst, uint16_t val)
{
SBUFPUSH(dst, val >> 8);
SBUFPUSH(dst, val >> 0);
}

void sbufWriteU24(sbuf_t *dst, uint32_t val)
{
SBUFPUSH(dst, val >> 0);
SBUFPUSH(dst, val >> 8);
SBUFPUSH(dst, val >> 16);
}

void sbufWriteU24BE(sbuf_t *dst, uint32_t val)
{
SBUFPUSH(dst, val >> 16);
SBUFPUSH(dst, val >> 8);
SBUFPUSH(dst, val >> 0);
}

void sbufWriteU32(sbuf_t *dst, uint32_t val)
{
sbufWriteU8(dst, val >> 0);
sbufWriteU8(dst, val >> 8);
sbufWriteU8(dst, val >> 16);
sbufWriteU8(dst, val >> 24);
SBUFPUSH(dst, val >> 0);
SBUFPUSH(dst, val >> 8);
SBUFPUSH(dst, val >> 16);
SBUFPUSH(dst, val >> 24);
}

void sbufWriteU32BE(sbuf_t *dst, uint32_t val)
{
SBUFPUSH(dst, val >> 24);
SBUFPUSH(dst, val >> 16);
SBUFPUSH(dst, val >> 8);
SBUFPUSH(dst, val >> 0);
}

void sbufWriteU64(sbuf_t *dst, uint64_t val)
{
sbufWriteU32(dst, (uint32_t)val);
sbufWriteU32(dst, (uint32_t)(val >> 32));
SBUFPUSH(dst, val >> 0);
SBUFPUSH(dst, val >> 8);
SBUFPUSH(dst, val >> 16);
SBUFPUSH(dst, val >> 24);
SBUFPUSH(dst, val >> 32);
SBUFPUSH(dst, val >> 40);
SBUFPUSH(dst, val >> 48);
SBUFPUSH(dst, val >> 56);
}

void sbufWriteU16BigEndian(sbuf_t *dst, uint16_t val)
void sbufWriteU64BE(sbuf_t *dst, uint64_t val)
{
sbufWriteU8(dst, val >> 8);
sbufWriteU8(dst, (uint8_t)val);
SBUFPUSH(dst, val >> 56);
SBUFPUSH(dst, val >> 48);
SBUFPUSH(dst, val >> 40);
SBUFPUSH(dst, val >> 32);
SBUFPUSH(dst, val >> 24);
SBUFPUSH(dst, val >> 16);
SBUFPUSH(dst, val >> 8);
SBUFPUSH(dst, val >> 0);
}

void sbufWriteU32BigEndian(sbuf_t *dst, uint32_t val)
void sbufWriteFloat(sbuf_t *dst, float val)
{
sbufWriteU8(dst, val >> 24);
sbufWriteU8(dst, val >> 16);
sbufWriteU8(dst, val >> 8);
sbufWriteU8(dst, (uint8_t)val);
sbufWriteU32(dst, REINTERPRET_CAST(val, uint32_t));
}


Expand All @@ -86,71 +125,95 @@ void sbufWriteData(sbuf_t *dst, const void *data, int len)

void sbufWriteString(sbuf_t *dst, const char *string)
{
sbufWriteData(dst, string, strlen(string));
while (*string) { SBUFPUSH(dst, *string++); }
}

void sbufWriteStringWithZeroTerminator(sbuf_t *dst, const char *string)
{
sbufWriteData(dst, string, strlen(string) + 1);
do { SBUFPUSH(dst, *string); } while (*string++);
}

uint8_t sbufReadU8(sbuf_t *src)
{
return *src->ptr++;
return SBUFPOP(src);
}

uint16_t sbufReadU16(sbuf_t *src)
{
uint16_t ret;
ret = sbufReadU8(src);
ret |= sbufReadU8(src) << 8;
uint32_t ret;
ret = SBUFPOP(src) << 0;
ret |= SBUFPOP(src) << 8;
return ret;
}

uint32_t sbufReadU32(sbuf_t *src)
uint16_t sbufReadU16BE(sbuf_t *src)
{
uint32_t ret;
ret = sbufReadU8(src);
ret |= sbufReadU8(src) << 8;
ret |= sbufReadU8(src) << 16;
ret |= sbufReadU8(src) << 24;
ret = SBUFPOP(src) << 8;
ret |= SBUFPOP(src) << 0;
return ret;
}

uint64_t sbufReadU64(sbuf_t *src)
uint32_t sbufReadU24(sbuf_t *src)
{
uint32_t ret;
ret = SBUFPOP(src) << 0;
ret |= SBUFPOP(src) << 8;
ret |= SBUFPOP(src) << 16;
return ret;
}

uint32_t sbufReadU24BE(sbuf_t *src)
{
uint32_t ret = sbufReadU32(src);
return (uint64_t)sbufReadU32(src) << 32 | ret;
uint32_t ret;
ret = SBUFPOP(src) << 16;
ret |= SBUFPOP(src) << 8;
ret |= SBUFPOP(src) << 0;
return ret;
}

void sbufReadData(sbuf_t *src, void *data, int len)
uint32_t sbufReadU32(sbuf_t *src)
{
memcpy(data, src->ptr, len);
uint32_t ret;
ret = SBUFPOP(src) << 0;
ret |= SBUFPOP(src) << 8;
ret |= SBUFPOP(src) << 16;
ret |= SBUFPOP(src) << 24;
return ret;
}

// reader - return bytes remaining in buffer
// writer - return available space
int sbufBytesRemaining(sbuf_t *buf)
uint32_t sbufReadU32BE(sbuf_t *src)
{
return buf->end - buf->ptr;
uint32_t ret;
ret = SBUFPOP(src) << 24;
ret |= SBUFPOP(src) << 16;
ret |= SBUFPOP(src) << 8;
ret |= SBUFPOP(src) << 0;
return ret;
}

uint8_t* sbufPtr(sbuf_t *buf)
uint64_t sbufReadU64(sbuf_t *src)
{
return buf->ptr;
uint64_t a = sbufReadU32(src);
uint64_t b = sbufReadU32(src);
return a | (b << 32);
}

const uint8_t* sbufConstPtr(const sbuf_t *buf)
uint64_t sbufReadU64BE(sbuf_t *src)
{
return buf->ptr;
uint64_t a = sbufReadU32(src);
uint64_t b = sbufReadU32(src);
return (a << 32) | b;
}

// advance buffer pointer
// reader - skip data
// writer - commit written data
void sbufAdvance(sbuf_t *buf, int size)
float sbufReadFloat(sbuf_t *src)
{
buf->ptr += size;
return REINTERPRET_CAST(sbufReadU32(src), float);
}

void sbufReadData(sbuf_t *src, void *data, int len)
{
memcpy(data, src->ptr, len);
}

// modifies streambuf so that written data are prepared for reading
Expand Down
96 changes: 77 additions & 19 deletions src/main/common/streambuf.h
View file
Original file line number Diff line number Diff line change
@@ -1,21 +1,18 @@
/*
* This file is part of Cleanflight and Betaflight.
* This file is part of Rotorflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software 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.
* Rotorflight 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 and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* Rotorflight 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 this software.
*
* If not, see <http://www.gnu.org/licenses/>.
* along with this software. If not, see <https://www.gnu.org/licenses/>.
*/

#pragma once
Expand All @@ -29,28 +26,89 @@ typedef struct sbuf_s {
uint8_t *end;
} sbuf_t;

sbuf_t *sbufInit(sbuf_t *sbuf, uint8_t *ptr, uint8_t *end);
/* Init function */
sbuf_t * sbufInit(sbuf_t *sbuf, uint8_t *ptr, uint8_t *end);

/* Write Unsigned Integers */
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 sbufWriteU24(sbuf_t *dst, uint32_t val);
void sbufWriteU64(sbuf_t *dst, uint64_t val);
void sbufWriteU16BigEndian(sbuf_t *dst, uint16_t val);
void sbufWriteU32BigEndian(sbuf_t *dst, uint32_t val);

/* Write Unsigned Integers in Big-Endian */
void sbufWriteU16BE(sbuf_t *dst, uint16_t val);
void sbufWriteU24BE(sbuf_t *dst, uint32_t val);
void sbufWriteU32BE(sbuf_t *dst, uint32_t val);
void sbufWriteU64BE(sbuf_t *dst, uint64_t val);

/* Compat */
static inline void sbufWriteU16BigEndian(sbuf_t *dst, uint16_t val) { sbufWriteU16BE(dst, val); }
static inline void sbufWriteU32BigEndian(sbuf_t *dst, uint32_t val) { sbufWriteU32BE(dst, val); }

/* Write Signed Integers */
static inline void sbufWriteS8(sbuf_t *dst, int8_t val) { sbufWriteU8(dst, val); }
static inline void sbufWriteS16(sbuf_t *dst, int16_t val) { sbufWriteU16(dst, val); }
static inline void sbufWriteS24(sbuf_t *dst, int32_t val) { sbufWriteU24(dst, val); }
static inline void sbufWriteS32(sbuf_t *dst, int32_t val) { sbufWriteU32(dst, val); }
static inline void sbufWriteS64(sbuf_t *dst, int64_t val) { sbufWriteU64(dst, val); }

/* Write Signed Integers in Big-Endian */
static inline void sbufWriteS16BE(sbuf_t *dst, int16_t val) { sbufWriteU16BE(dst, val); }
static inline void sbufWriteS24BE(sbuf_t *dst, int32_t val) { sbufWriteU24BE(dst, val); }
static inline void sbufWriteS32BE(sbuf_t *dst, int32_t val) { sbufWriteU32BE(dst, val); }
static inline void sbufWriteS64BE(sbuf_t *dst, int64_t val) { sbufWriteU64BE(dst, val); }

/* Write Float */
void sbufWriteFloat(sbuf_t *dst, float val);

/* Write Data and strings */
void sbufFill(sbuf_t *dst, uint8_t data, int len);
void sbufWriteData(sbuf_t *dst, const void *data, int len);
void sbufWriteString(sbuf_t *dst, const char *string);
void sbufWriteStringWithZeroTerminator(sbuf_t *dst, const char *string);

/* Read Unsigned Integers */
uint8_t sbufReadU8(sbuf_t *src);
uint16_t sbufReadU16(sbuf_t *src);
uint32_t sbufReadU32(sbuf_t *src);
uint64_t sbufReadU64(sbuf_t *src);

/* Read Big-Ending Unsigned Integers */
uint8_t sbufReadU8BE(sbuf_t *src);
uint16_t sbufReadU16BE(sbuf_t *src);
uint32_t sbufReadU32BE(sbuf_t *src);
uint64_t sbufReadU64BE(sbuf_t *src);

/* Read Signed Integers */
static inline int8_t sbufReadS8(sbuf_t *src) { return sbufReadU8(src); }
static inline int16_t sbufReadS16(sbuf_t *src) { return sbufReadU16(src); }
static inline int32_t sbufReadS32(sbuf_t *src) { return sbufReadU32(src); }
static inline int64_t sbufReadS64(sbuf_t *src) { return sbufReadU64(src); }

/* Read Big-Endian Signed Integers */
static inline int16_t sbufReadS16BE(sbuf_t *src) { return sbufReadU16BE(src); }
static inline int32_t sbufReadS32BE(sbuf_t *src) { return sbufReadU32BE(src); }
static inline int64_t sbufReadS64BE(sbuf_t *src) { return sbufReadU64BE(src); }

/* Read float */
float sbufReadFloat(sbuf_t *src);

/* Read Data and Strings */
void sbufReadData(sbuf_t *dst, void *data, int len);

int sbufBytesRemaining(sbuf_t *buf);
uint8_t* sbufPtr(sbuf_t *buf);
const uint8_t* sbufConstPtr(const sbuf_t *buf);
void sbufAdvance(sbuf_t *buf, int size);
/* Space left in the buffer */
static inline int sbufBytesRemaining(sbuf_t *buf) { return (buf->end - buf->ptr); }

/* Get buffer pointer */
static inline uint8_t * sbufPtr(sbuf_t *buf) { return buf->ptr; }
static inline const uint8_t * sbufConstPtr(const sbuf_t *buf) { return buf->ptr; }

/* Move pointer forward */
static inline void sbufAdvance(sbuf_t *buf, int size) { buf->ptr += size; }

/* Set to another position */
static inline void sbufReset(sbuf_t *buf, uint8_t *ptr) { buf->ptr = ptr; }

/* Prepare buffer for reading */
void sbufSwitchToReader(sbuf_t *buf, uint8_t * base);
Loading

0 comments on commit a535da9

Please sign in to comment.