Tasmota/sonoff/support_static_buffer.ino

213 lines
6.0 KiB
C++

/*
support_buffer.ino - Static binary buffer for Zigbee
Copyright (C) 2019 Theo Arends and Stephan Hadinger
This program 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.
This program 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 program. If not, see <http://www.gnu.org/licenses/>.
*/
typedef struct SBuffer_impl {
uint16_t size; // size in bytes of the buffer
uint16_t len; // current size of the data in buffer. Invariant: len <= size
uint8_t buf[]; // the actual data
} SBuffer_impl;
typedef class SBuffer {
protected:
SBuffer(void) {
// unused empty constructor except from subclass
}
public:
SBuffer(const size_t size) {
_buf = (SBuffer_impl*) new char[size+4]; // add 4 bytes for size and len
_buf->size = size;
_buf->len = 0;
//*((uint32_t*)_buf) = size; // writing both size and len=0 in a single 32 bits write
}
inline size_t getSize(void) const { return _buf->size; }
inline size_t size(void) const { return _buf->size; }
inline size_t getLen(void) const { return _buf->len; }
inline size_t len(void) const { return _buf->len; }
inline uint8_t *getBuffer(void) const { return _buf->buf; }
inline uint8_t *buf(size_t i = 0) const { return &_buf->buf[i]; }
inline char *charptr(size_t i = 0) const { return (char*) &_buf->buf[i]; }
virtual ~SBuffer(void) {
delete[] _buf;
}
inline void setLen(const size_t len) {
uint16_t old_len = _buf->len;
_buf->len = (len <= _buf->size) ? len : _buf->size;
if (old_len < _buf->len) {
memset((void*) &_buf->buf[old_len], 0, _buf->len - old_len);
}
}
void set8(const size_t offset, const uint8_t data) {
if (offset < _buf->len) {
_buf->buf[offset] = data;
}
}
size_t add8(const uint8_t data) { // append 8 bits value
if (_buf->len < _buf->size) { // do we have room for 1 byte
_buf->buf[_buf->len++] = data;
}
return _buf->len;
}
size_t add16(const uint16_t data) { // append 16 bits value
if (_buf->len < _buf->size - 1) { // do we have room for 2 bytes
_buf->buf[_buf->len++] = data;
_buf->buf[_buf->len++] = data >> 8;
}
return _buf->len;
}
size_t add32(const uint32_t data) { // append 32 bits value
if (_buf->len < _buf->size - 3) { // do we have room for 2 bytes
_buf->buf[_buf->len++] = data;
_buf->buf[_buf->len++] = data >> 8;
_buf->buf[_buf->len++] = data >> 16;
_buf->buf[_buf->len++] = data >> 24;
}
return _buf->len;
}
size_t addBuffer(const SBuffer &buf2) {
if (len() + buf2.len() <= size()) {
for (uint32_t i = 0; i < buf2.len(); i++) {
_buf->buf[_buf->len++] = buf2.buf()[i];
}
}
return _buf->len;
}
size_t addBuffer(const uint8_t *buf2, size_t len2) {
if (len() + len2 <= size()) {
for (uint32_t i = 0; i < len2; i++) {
_buf->buf[_buf->len++] = pgm_read_byte(&buf2[i]);
}
}
return _buf->len;
}
size_t addBuffer(const char *buf2, size_t len2) {
if (len() + len2 <= size()) {
for (uint32_t i = 0; i < len2; i++) {
_buf->buf[_buf->len++] = pgm_read_byte(&buf2[i]);
}
}
return _buf->len;
}
uint8_t get8(size_t offset) const {
if (offset < _buf->len) {
return _buf->buf[offset];
} else {
return 0;
}
}
uint8_t read8(const size_t offset) const {
if (offset < len()) {
return _buf->buf[offset];
}
return 0;
}
uint16_t get16(const size_t offset) const {
if (offset < len() - 1) {
return _buf->buf[offset] | (_buf->buf[offset+1] << 8);
}
return 0;
}
uint32_t get32(const size_t offset) const {
if (offset < len() - 3) {
return _buf->buf[offset] | (_buf->buf[offset+1] << 8) |
(_buf->buf[offset+2] << 16) | (_buf->buf[offset+3] << 24);
}
return 0;
}
uint64_t get64(const size_t offset) const {
if (offset < len() - 7) {
return (uint64_t)_buf->buf[offset] | ((uint64_t)_buf->buf[offset+1] << 8) |
((uint64_t)_buf->buf[offset+2] << 16) | ((uint64_t)_buf->buf[offset+3] << 24) |
((uint64_t)_buf->buf[offset+4] << 32) | ((uint64_t)_buf->buf[offset+5] << 40) |
((uint64_t)_buf->buf[offset+6] << 48) | ((uint64_t)_buf->buf[offset+7] << 56);
}
return 0;
}
SBuffer subBuffer(const size_t start, size_t len) const {
if (start >= _buf->len) {
len = 0;
} else if (start + len > _buf->len) {
len = _buf->len - start;
}
SBuffer buf2(len);
memcpy(buf2.buf(), buf()+start, len);
buf2._buf->len = len;
return buf2;
}
static SBuffer SBufferFromHex(const char *hex, size_t len) {
size_t buf_len = (len + 3) / 2;
SBuffer buf2(buf_len);
uint8_t val;
for (; len > 1; len -= 2) {
val = asc2byte(*hex++) << 4;
val |= asc2byte(*hex++);
buf2.add8(val);
}
return buf2;
}
protected:
static uint8_t asc2byte(char chr) {
uint8_t rVal = 0;
if (isdigit(chr)) { rVal = chr - '0'; }
else if (chr >= 'A' && chr <= 'F') { rVal = chr + 10 - 'A'; }
else if (chr >= 'a' && chr <= 'f') { rVal = chr + 10 - 'a'; }
return rVal;
}
static void unHex(const char* in, uint8_t *out, size_t len) {
}
protected:
SBuffer_impl * _buf;
} SBuffer;
typedef class PreAllocatedSBuffer : public SBuffer {
public:
PreAllocatedSBuffer(const size_t size, void * buffer) {
_buf = (SBuffer_impl*) buffer;
_buf->size = size - 4;
_buf->len = 0;
}
~PreAllocatedSBuffer(void) {
// don't deallocate
_buf = nullptr;
}
} PreAllocatedSBuffer;