From 13b92d37531957296eced37ed18d54c2cff643ae Mon Sep 17 00:00:00 2001 From: me-no-dev Date: Fri, 17 Feb 2023 11:30:14 +0200 Subject: [PATCH 1/6] Rework cbuf to use FreeRTOS Ringbuffer --- cores/esp32/cbuf.cpp | 241 +++++++++++++++++++++++++++---------------- cores/esp32/cbuf.h | 37 +++---- 2 files changed, 163 insertions(+), 115 deletions(-) diff --git a/cores/esp32/cbuf.cpp b/cores/esp32/cbuf.cpp index ef7370a8a07..0758678db61 100644 --- a/cores/esp32/cbuf.cpp +++ b/cores/esp32/cbuf.cpp @@ -20,177 +20,238 @@ #include "cbuf.h" +#if CONFIG_DISABLE_HAL_LOCKS +#define CBUF_MUTEX_CREATE() +#define CBUF_MUTEX_LOCK() +#define CBUF_MUTEX_UNLOCK() +#define CBUF_MUTEX_DELETE() +#else +#define CBUF_MUTEX_CREATE() if(_lock == NULL){_lock = xSemaphoreCreateMutex(); if(_lock == NULL){log_e("failed to create mutex");}} +#define CBUF_MUTEX_LOCK() if(_lock != NULL){xSemaphoreTakeRecursive(_lock, portMAX_DELAY);} +#define CBUF_MUTEX_UNLOCK() if(_lock != NULL){xSemaphoreGiveRecursive(_lock);} +#define CBUF_MUTEX_DELETE() if(_lock != NULL){SemaphoreHandle_t l = _lock; _lock = NULL; vSemaphoreDelete(l);} +#endif + cbuf::cbuf(size_t size) : - next(NULL), _size(size+1), _buf(new char[size+1]), _bufend(_buf + size + 1), _begin(_buf), _end(_begin) + next(NULL), _buf(xRingbufferCreate(size, RINGBUF_TYPE_BYTEBUF)) { + if(_buf == NULL) { + log_e("failed to allocate ring buffer"); + } + CBUF_MUTEX_CREATE(); } cbuf::~cbuf() { - delete[] _buf; + CBUF_MUTEX_LOCK(); + if(_buf != NULL){ + RingbufHandle_t b = _buf; + _buf = NULL; + vRingbufferDelete(b); + } + CBUF_MUTEX_UNLOCK(); + CBUF_MUTEX_DELETE(); } size_t cbuf::resizeAdd(size_t addSize) { - return resize(_size + addSize); + return resize(size() + addSize); } size_t cbuf::resize(size_t newSize) { + CBUF_MUTEX_LOCK(); + size_t _size = size(); + if(newSize == _size) { + return _size; + } - size_t bytes_available = available(); - newSize += 1; // not lose any data // if data can be lost use remove or flush before resize - if((newSize < bytes_available) || (newSize == _size)) { + size_t bytes_available = available(); + if(newSize < bytes_available) { + CBUF_MUTEX_UNLOCK(); + log_e("new size is less than the currently available data size"); return _size; } - char *newbuf = new char[newSize]; - char *oldbuf = _buf; - - if(!newbuf) { + RingbufHandle_t newbuf = xRingbufferCreate(newSize, RINGBUF_TYPE_BYTEBUF); + if(newbuf == NULL) { + CBUF_MUTEX_UNLOCK(); + log_e("failed to allocate new ring buffer"); return _size; } - if(_buf) { - read(newbuf, bytes_available); - memset((newbuf + bytes_available), 0x00, (newSize - bytes_available)); + if(_buf != NULL) { + if(bytes_available){ + char * old_data = (char *)malloc(bytes_available); + if(old_data == NULL){ + vRingbufferDelete(newbuf); + CBUF_MUTEX_UNLOCK(); + log_e("failed to allocate temporary buffer"); + return _size; + } + bytes_available = read(old_data, bytes_available); + if(!bytes_available){ + free(old_data); + vRingbufferDelete(newbuf); + CBUF_MUTEX_UNLOCK(); + log_e("failed to read previous data"); + return _size; + } + if(xRingbufferSend(newbuf, (void*)old_data, bytes_available, 0) != pdTRUE){ + write(old_data, bytes_available); + free(old_data); + vRingbufferDelete(newbuf); + CBUF_MUTEX_UNLOCK(); + log_e("failed to restore previous data"); + return _size; + } + free(old_data); + } + + RingbufHandle_t b = _buf; + _buf = newbuf; + vRingbufferDelete(b); + } else { + _buf = newbuf; } - - _begin = newbuf; - _end = newbuf + bytes_available; - _bufend = newbuf + newSize; - _size = newSize; - - _buf = newbuf; - delete[] oldbuf; - - return _size; + CBUF_MUTEX_UNLOCK(); + return newSize; } size_t cbuf::available() const { - if(_end >= _begin) { - return _end - _begin; + size_t available = 0; + if(_buf != NULL){ + vRingbufferGetInfo(_buf, NULL, NULL, NULL, NULL, (UBaseType_t *)&available); } - return _size - (_begin - _end); + return available; } size_t cbuf::size() { + size_t _size = 0; + if(_buf != NULL){ + _size = xRingbufferGetMaxItemSize(_buf); + } return _size; } size_t cbuf::room() const { - if(_end >= _begin) { - return _size - (_end - _begin) - 1; + size_t _room = 0; + if(_buf != NULL){ + _room = xRingbufferGetCurFreeSize(_buf); } - return _begin - _end - 1; + return _room; } -int cbuf::peek() +bool cbuf::empty() const { - if(empty()) { - return -1; - } + return available() == 0; +} - return static_cast(*_begin); +bool cbuf::full() const +{ + return room() == 0; +} + +int cbuf::peek() +{ + return -1; } size_t cbuf::peek(char *dst, size_t size) { - size_t bytes_available = available(); - size_t size_to_read = (size < bytes_available) ? size : bytes_available; - size_t size_read = size_to_read; - char * begin = _begin; - if(_end < _begin && size_to_read > (size_t) (_bufend - _begin)) { - size_t top_size = _bufend - _begin; - memcpy(dst, _begin, top_size); - begin = _buf; - size_to_read -= top_size; - dst += top_size; - } - memcpy(dst, begin, size_to_read); - return size_read; + return 0; } int cbuf::read() { - if(empty()) { + char result = 0; + if(!read(&result, 1)){ return -1; } - - char result = *_begin; - _begin = wrap_if_bufend(_begin + 1); return static_cast(result); } size_t cbuf::read(char* dst, size_t size) { + CBUF_MUTEX_LOCK(); size_t bytes_available = available(); + if(!bytes_available || !size){ + CBUF_MUTEX_UNLOCK(); + return 0; + } size_t size_to_read = (size < bytes_available) ? size : bytes_available; - size_t size_read = size_to_read; - if(_end < _begin && size_to_read > (size_t) (_bufend - _begin)) { - size_t top_size = _bufend - _begin; - memcpy(dst, _begin, top_size); - _begin = _buf; - size_to_read -= top_size; - dst += top_size; + size_t size_read = 0; + size_t received_size = 0; + uint8_t *received_buff = (uint8_t *)xRingbufferReceiveUpTo(_buf, &received_size, 0, size_to_read); + if (received_buff != NULL) { + if(dst != NULL){ + memcpy(dst, received_buff, received_size); + } + vRingbufferReturnItem(rx_ring_buf, received_buff); + size_read = received_size; + size_to_read -= received_size; + // wrap around data + if(size_to_read){ + received_size = 0; + received_buff = (uint8_t *)xRingbufferReceiveUpTo(_buf, &received_size, 0, size_to_read); + if (received_buff != NULL) { + if(dst != NULL){ + memcpy(dst+size_read, received_buff, received_size); + } + vRingbufferReturnItem(rx_ring_buf, received_buff); + size_read += received_size; + } else { + log_e("failed to read wrap around data from ring buffer"); + } + } + } else { + log_e("failed to read from ring buffer"); } - memcpy(dst, _begin, size_to_read); - _begin = wrap_if_bufend(_begin + size_to_read); + CBUF_MUTEX_UNLOCK(); return size_read; } size_t cbuf::write(char c) { - if(full()) { - return 0; - } - - *_end = c; - _end = wrap_if_bufend(_end + 1); - return 1; + return write(&c, 1); } size_t cbuf::write(const char* src, size_t size) { + CBUF_MUTEX_LOCK(); size_t bytes_available = room(); + if(!bytes_available || !size){ + CBUF_MUTEX_UNLOCK(); + return 0; + } size_t size_to_write = (size < bytes_available) ? size : bytes_available; - size_t size_written = size_to_write; - if(_end >= _begin && size_to_write > (size_t) (_bufend - _end)) { - size_t top_size = _bufend - _end; - memcpy(_end, src, top_size); - _end = _buf; - size_to_write -= top_size; - src += top_size; + if(xRingbufferSend(rx_ring_buf, (void*)src, size_to_write, 0) != pdTRUE){ + CBUF_MUTEX_UNLOCK(); + log_e("failed to write to ring buffer"); + return 0; } - memcpy(_end, src, size_to_write); - _end = wrap_if_bufend(_end + size_to_write); - return size_written; + CBUF_MUTEX_UNLOCK(); + return size_to_write; } void cbuf::flush() { - _begin = _buf; - _end = _buf; + read(NULL, available()); } size_t cbuf::remove(size_t size) { + CBUF_MUTEX_LOCK(); size_t bytes_available = available(); - if(size >= bytes_available) { - flush(); - return 0; - } - size_t size_to_remove = (size < bytes_available) ? size : bytes_available; - if(_end < _begin && size_to_remove > (size_t) (_bufend - _begin)) { - size_t top_size = _bufend - _begin; - _begin = _buf; - size_to_remove -= top_size; + if(bytes_available && size){ + size_t size_to_remove = (size < bytes_available) ? size : bytes_available; + bytes_available -= read(NULL, size_to_remove); } - _begin = wrap_if_bufend(_begin + size_to_remove); - return available(); + CBUF_MUTEX_UNLOCK(); + return bytes_available; } diff --git a/cores/esp32/cbuf.h b/cores/esp32/cbuf.h index 490352e3202..4334594791f 100644 --- a/cores/esp32/cbuf.h +++ b/cores/esp32/cbuf.h @@ -18,12 +18,15 @@ Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA */ -#ifndef __cbuf_h -#define __cbuf_h +#pragma once #include #include #include +#include "sdkconfig.h" +#include "freertos/FreeRTOS.h" +#include "freertos/ringbuf.h" +#include "freertos/semphr.h" class cbuf { @@ -33,20 +36,12 @@ class cbuf size_t resizeAdd(size_t addSize); size_t resize(size_t newSize); + size_t available() const; size_t size(); - size_t room() const; - - inline bool empty() const - { - return _begin == _end; - } - - inline bool full() const - { - return wrap_if_bufend(_end + 1) == _begin; - } + bool empty() const; + bool full() const; int peek(); size_t peek(char *dst, size_t size); @@ -63,17 +58,9 @@ class cbuf cbuf *next; protected: - inline char* wrap_if_bufend(char* ptr) const - { - return (ptr == _bufend) ? _buf : ptr; - } - - size_t _size; - char* _buf; - const char* _bufend; - char* _begin; - char* _end; + RingbufHandle_t _buf; +#if !CONFIG_DISABLE_HAL_LOCKS + SemaphoreHandle_t _lock; +#endif }; - -#endif//__cbuf_h From f4a48648e0a043edc5cbbdc2c7d09df920955bc1 Mon Sep 17 00:00:00 2001 From: me-no-dev Date: Fri, 17 Feb 2023 11:37:21 +0200 Subject: [PATCH 2/6] Update cbuf.cpp --- cores/esp32/cbuf.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/cores/esp32/cbuf.cpp b/cores/esp32/cbuf.cpp index 0758678db61..fa758f15292 100644 --- a/cores/esp32/cbuf.cpp +++ b/cores/esp32/cbuf.cpp @@ -19,6 +19,7 @@ */ #include "cbuf.h" +#include "esp32-hal-log.h.h" #if CONFIG_DISABLE_HAL_LOCKS #define CBUF_MUTEX_CREATE() @@ -192,7 +193,7 @@ size_t cbuf::read(char* dst, size_t size) if(dst != NULL){ memcpy(dst, received_buff, received_size); } - vRingbufferReturnItem(rx_ring_buf, received_buff); + vRingbufferReturnItem(_buf, received_buff); size_read = received_size; size_to_read -= received_size; // wrap around data @@ -203,7 +204,7 @@ size_t cbuf::read(char* dst, size_t size) if(dst != NULL){ memcpy(dst+size_read, received_buff, received_size); } - vRingbufferReturnItem(rx_ring_buf, received_buff); + vRingbufferReturnItem(_buf, received_buff); size_read += received_size; } else { log_e("failed to read wrap around data from ring buffer"); @@ -230,7 +231,7 @@ size_t cbuf::write(const char* src, size_t size) return 0; } size_t size_to_write = (size < bytes_available) ? size : bytes_available; - if(xRingbufferSend(rx_ring_buf, (void*)src, size_to_write, 0) != pdTRUE){ + if(xRingbufferSend(_buf, (void*)src, size_to_write, 0) != pdTRUE){ CBUF_MUTEX_UNLOCK(); log_e("failed to write to ring buffer"); return 0; From c089b7a18821109db3fd80973b3e4a17b19999df Mon Sep 17 00:00:00 2001 From: me-no-dev Date: Fri, 17 Feb 2023 11:43:27 +0200 Subject: [PATCH 3/6] Fix typo --- cores/esp32/cbuf.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cores/esp32/cbuf.cpp b/cores/esp32/cbuf.cpp index fa758f15292..42903d397b9 100644 --- a/cores/esp32/cbuf.cpp +++ b/cores/esp32/cbuf.cpp @@ -19,7 +19,7 @@ */ #include "cbuf.h" -#include "esp32-hal-log.h.h" +#include "esp32-hal-log.h" #if CONFIG_DISABLE_HAL_LOCKS #define CBUF_MUTEX_CREATE() From 76710cfe294fc305de4fd4c957b4a2090e645a0e Mon Sep 17 00:00:00 2001 From: Lucas Saavedra Vaz <32426024+lucasssvaz@users.noreply.github.com> Date: Fri, 2 Feb 2024 13:58:49 -0300 Subject: [PATCH 4/6] Initialize with NULL --- cores/esp32/cbuf.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cores/esp32/cbuf.h b/cores/esp32/cbuf.h index 4334594791f..57e1398b73c 100644 --- a/cores/esp32/cbuf.h +++ b/cores/esp32/cbuf.h @@ -58,9 +58,9 @@ class cbuf cbuf *next; protected: - RingbufHandle_t _buf; + RingbufHandle_t _buf = NULL; #if !CONFIG_DISABLE_HAL_LOCKS - SemaphoreHandle_t _lock; + SemaphoreHandle_t _lock = NULL; #endif }; From d5d970f90d72b61f9e927ce61a12b954e9550e54 Mon Sep 17 00:00:00 2001 From: Lucas Saavedra Vaz <32426024+lucasssvaz@users.noreply.github.com> Date: Mon, 5 Feb 2024 12:04:33 -0300 Subject: [PATCH 5/6] Implement peek method --- cores/esp32/cbuf.cpp | 89 +++++++++++++++++++++++++++++--------------- cores/esp32/cbuf.h | 3 +- 2 files changed, 62 insertions(+), 30 deletions(-) diff --git a/cores/esp32/cbuf.cpp b/cores/esp32/cbuf.cpp index 42903d397b9..59c83b46d3c 100644 --- a/cores/esp32/cbuf.cpp +++ b/cores/esp32/cbuf.cpp @@ -34,7 +34,9 @@ #endif cbuf::cbuf(size_t size) : - next(NULL), _buf(xRingbufferCreate(size, RINGBUF_TYPE_BYTEBUF)) + next(NULL), + has_peek(false), + _buf(xRingbufferCreate(size, RINGBUF_TYPE_BYTEBUF)) { if(_buf == NULL) { log_e("failed to allocate ring buffer"); @@ -127,6 +129,7 @@ size_t cbuf::available() const if(_buf != NULL){ vRingbufferGetInfo(_buf, NULL, NULL, NULL, NULL, (UBaseType_t *)&available); } + if (has_peek) available++; return available; } @@ -160,12 +163,24 @@ bool cbuf::full() const int cbuf::peek() { - return -1; -} + if (!available()) { + return -1; + } -size_t cbuf::peek(char *dst, size_t size) -{ - return 0; + int c; + + CBUF_MUTEX_LOCK(); + if (has_peek) { + c = peek_byte; + } else { + c = read(); + if (c >= 0) { + has_peek = true; + peek_byte = c; + } + } + CBUF_MUTEX_UNLOCK(); + return c; } int cbuf::read() @@ -185,34 +200,50 @@ size_t cbuf::read(char* dst, size_t size) CBUF_MUTEX_UNLOCK(); return 0; } - size_t size_to_read = (size < bytes_available) ? size : bytes_available; - size_t size_read = 0; - size_t received_size = 0; - uint8_t *received_buff = (uint8_t *)xRingbufferReceiveUpTo(_buf, &received_size, 0, size_to_read); - if (received_buff != NULL) { - if(dst != NULL){ - memcpy(dst, received_buff, received_size); + + if (has_peek) { + if (dst != NULL) { + *dst++ = peek_byte; } - vRingbufferReturnItem(_buf, received_buff); - size_read = received_size; - size_to_read -= received_size; - // wrap around data - if(size_to_read){ - received_size = 0; - received_buff = (uint8_t *)xRingbufferReceiveUpTo(_buf, &received_size, 0, size_to_read); - if (received_buff != NULL) { - if(dst != NULL){ - memcpy(dst+size_read, received_buff, received_size); + size--; + } + + size_t size_read = 0; + if (size) { + size_t received_size = 0; + size_t size_to_read = (size < bytes_available) ? size : bytes_available; + uint8_t *received_buff = (uint8_t *)xRingbufferReceiveUpTo(_buf, &received_size, 0, size_to_read); + if (received_buff != NULL) { + if(dst != NULL){ + memcpy(dst, received_buff, received_size); + } + vRingbufferReturnItem(_buf, received_buff); + size_read = received_size; + size_to_read -= received_size; + // wrap around data + if(size_to_read){ + received_size = 0; + received_buff = (uint8_t *)xRingbufferReceiveUpTo(_buf, &received_size, 0, size_to_read); + if (received_buff != NULL) { + if(dst != NULL){ + memcpy(dst+size_read, received_buff, received_size); + } + vRingbufferReturnItem(_buf, received_buff); + size_read += received_size; + } else { + log_e("failed to read wrap around data from ring buffer"); } - vRingbufferReturnItem(_buf, received_buff); - size_read += received_size; - } else { - log_e("failed to read wrap around data from ring buffer"); } + } else { + log_e("failed to read from ring buffer"); } - } else { - log_e("failed to read from ring buffer"); } + + if (has_peek) { + has_peek = false; + size_read++; + } + CBUF_MUTEX_UNLOCK(); return size_read; } diff --git a/cores/esp32/cbuf.h b/cores/esp32/cbuf.h index 57e1398b73c..29e11efb83e 100644 --- a/cores/esp32/cbuf.h +++ b/cores/esp32/cbuf.h @@ -44,7 +44,6 @@ class cbuf bool full() const; int peek(); - size_t peek(char *dst, size_t size); int read(); size_t read(char* dst, size_t size); @@ -56,6 +55,8 @@ class cbuf size_t remove(size_t size); cbuf *next; + bool has_peek; + uint8_t peek_byte; protected: RingbufHandle_t _buf = NULL; From d5656983244a8431d35460d08b8665e5e5544c13 Mon Sep 17 00:00:00 2001 From: Lucas Saavedra Vaz <32426024+lucasssvaz@users.noreply.github.com> Date: Wed, 7 Feb 2024 10:09:47 -0300 Subject: [PATCH 6/6] Add initializer --- cores/esp32/cbuf.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/cores/esp32/cbuf.cpp b/cores/esp32/cbuf.cpp index 59c83b46d3c..4a110fd732a 100644 --- a/cores/esp32/cbuf.cpp +++ b/cores/esp32/cbuf.cpp @@ -36,6 +36,7 @@ cbuf::cbuf(size_t size) : next(NULL), has_peek(false), + peek_byte(0), _buf(xRingbufferCreate(size, RINGBUF_TYPE_BYTEBUF)) { if(_buf == NULL) {