diff --git a/Makefile b/Makefile index e7f263c68..df4afde79 100644 --- a/Makefile +++ b/Makefile @@ -288,6 +288,7 @@ COMMON_SRC = build_config.c \ drivers/sound_beeper.c \ drivers/system.c \ drivers/gyro_sync.c \ + drivers/buf_writer.c \ io/beeper.c \ io/rc_controls.c \ io/rc_curves.c \ diff --git a/src/main/drivers/buf_writer.c b/src/main/drivers/buf_writer.c new file mode 100644 index 000000000..386111b1c --- /dev/null +++ b/src/main/drivers/buf_writer.c @@ -0,0 +1,47 @@ +/* + * 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 "buf_writer.h" + +bufWriter_t *bufWriterInit(uint8_t *b, int total_size, bufWrite_t writer, void *arg) +{ + bufWriter_t *buf = (bufWriter_t *)b; + buf->writer = writer; + buf->arg = arg; + buf->at = 0; + buf->capacity = total_size - sizeof(*buf); + + return buf; +} + +void bufWriterAppend(bufWriter_t *b, uint8_t ch) +{ + b->data[b->at++] = ch; + if (b->at >= b->capacity) { + bufWriterFlush(b); + } +} + +void bufWriterFlush(bufWriter_t *b) +{ + if (b->at != 0) { + b->writer(b->arg, b->data, b->at); + b->at = 0; + } +} diff --git a/src/main/drivers/buf_writer.h b/src/main/drivers/buf_writer.h new file mode 100644 index 000000000..93bef6efa --- /dev/null +++ b/src/main/drivers/buf_writer.h @@ -0,0 +1,38 @@ +/* + * 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 + +// Called to flush the buffer. +typedef void (*bufWrite_t)(void *arg, void *data, int count); + +typedef struct bufWriter_s { + bufWrite_t writer; + void *arg; + uint8_t capacity; + uint8_t at; + uint8_t data[]; +} bufWriter_t; + +// Initialise a block of memory as a buffered writer. +// +// b should be sizeof(bufWriter_t) + the number of bytes to buffer. +// total_size should be the total size of b. +// +bufWriter_t *bufWriterInit(uint8_t *b, int total_size, bufWrite_t writer, void *p); +void bufWriterAppend(bufWriter_t *b, uint8_t ch); +void bufWriterFlush(bufWriter_t *b);