356 lines
9.6 KiB
C++
356 lines
9.6 KiB
C++
#include "pxtbase.h"
|
|
#include <limits.h>
|
|
|
|
using namespace std;
|
|
|
|
//% indexerGet=BufferMethods::getByte indexerSet=BufferMethods::setByte
|
|
namespace BufferMethods {
|
|
//%
|
|
uint8_t *getBytes(Buffer buf) {
|
|
return buf->data;
|
|
}
|
|
|
|
//%
|
|
int getByte(Buffer buf, int off) {
|
|
if (buf && 0 <= off && off < buf->length)
|
|
return buf->data[off];
|
|
return 0;
|
|
}
|
|
|
|
//%
|
|
void setByte(Buffer buf, int off, int v) {
|
|
if (buf && 0 <= off && off < buf->length)
|
|
buf->data[off] = v;
|
|
}
|
|
|
|
int writeBuffer(Buffer buf, int dstOffset, Buffer src, int srcOffset = 0, int length = -1) {
|
|
if (length < 0)
|
|
length = src->length;
|
|
|
|
if (srcOffset < 0 || dstOffset < 0 || dstOffset > buf->length)
|
|
return -1;
|
|
|
|
length = min(src->length - srcOffset, buf->length - dstOffset);
|
|
|
|
if (length < 0)
|
|
return -1;
|
|
|
|
if (buf == src) {
|
|
memmove(buf->data + dstOffset, src->data + srcOffset, length);
|
|
} else {
|
|
memcpy(buf->data + dstOffset, src->data + srcOffset, length);
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* Write a number in specified format in the buffer.
|
|
*/
|
|
//%
|
|
void setNumber(Buffer buf, NumberFormat format, int offset, TNumber value) {
|
|
if (offset < 0)
|
|
return;
|
|
setNumberCore(buf->data + offset, buf->length - offset, format, value);
|
|
}
|
|
|
|
/**
|
|
* Read a number in specified format from the buffer.
|
|
*/
|
|
//%
|
|
TNumber getNumber(Buffer buf, NumberFormat format, int offset) {
|
|
if (offset < 0)
|
|
return fromInt(0);
|
|
return getNumberCore(buf->data + offset, buf->length - offset, format);
|
|
}
|
|
|
|
/** Returns the length of a Buffer object. */
|
|
//% property
|
|
int length(Buffer s) {
|
|
return s->length;
|
|
}
|
|
|
|
/**
|
|
* Fill (a fragment) of the buffer with given value.
|
|
*/
|
|
//%
|
|
void fill(Buffer buf, int value, int offset = 0, int length = -1) {
|
|
if (offset < 0 || offset > buf->length)
|
|
return; // DEVICE_INVALID_PARAMETER;
|
|
if (length < 0)
|
|
length = buf->length;
|
|
length = min(length, buf->length - offset);
|
|
memset(buf->data + offset, value, length);
|
|
}
|
|
|
|
/**
|
|
* Return a copy of a fragment of a buffer.
|
|
*/
|
|
//%
|
|
Buffer slice(Buffer buf, int offset = 0, int length = -1) {
|
|
offset = min((int)buf->length, offset);
|
|
if (length < 0)
|
|
length = buf->length;
|
|
length = min(length, buf->length - offset);
|
|
return mkBuffer(buf->data + offset, length);
|
|
}
|
|
|
|
/**
|
|
* Shift buffer left in place, with zero padding.
|
|
* @param offset number of bytes to shift; use negative value to shift right
|
|
* @param start start offset in buffer. Default is 0.
|
|
* @param length number of elements in buffer. If negative, length is set as the buffer length minus
|
|
* start. eg: -1
|
|
*/
|
|
//%
|
|
void shift(Buffer buf, int offset, int start = 0, int length = -1) {
|
|
if (length < 0)
|
|
length = buf->length - start;
|
|
if (start < 0 || start + length > buf->length || start + length < start || length == 0 ||
|
|
offset == 0 || offset == INT_MIN)
|
|
return;
|
|
if (offset <= -length || offset >= length) {
|
|
fill(buf, 0);
|
|
return;
|
|
}
|
|
|
|
uint8_t *data = buf->data + start;
|
|
if (offset < 0) {
|
|
offset = -offset;
|
|
memmove(data + offset, data, length - offset);
|
|
memset(data, 0, offset);
|
|
} else {
|
|
length = length - offset;
|
|
memmove(data, data + offset, length);
|
|
memset(data + length, 0, offset);
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Convert a buffer to its hexadecimal representation.
|
|
*/
|
|
//%
|
|
String toHex(Buffer buf) {
|
|
const char *hex = "0123456789abcdef";
|
|
auto res = mkString(NULL, buf->length * 2);
|
|
for (int i = 0; i < buf->length; ++i) {
|
|
res->data[i << 1] = hex[buf->data[i] >> 4];
|
|
res->data[(i << 1) + 1] = hex[buf->data[i] & 0xf];
|
|
}
|
|
return res;
|
|
}
|
|
|
|
/**
|
|
* Rotate buffer left in place.
|
|
* @param offset number of bytes to shift; use negative value to shift right
|
|
* @param start start offset in buffer. Default is 0.
|
|
* @param length number of elements in buffer. If negative, length is set as the buffer length minus
|
|
* start. eg: -1
|
|
*/
|
|
//%
|
|
void rotate(Buffer buf, int offset, int start = 0, int length = -1) {
|
|
if (length < 0)
|
|
length = buf->length - start;
|
|
if (start < 0 || start + length > buf->length || start + length < start || length == 0 ||
|
|
offset == 0 || offset == INT_MIN)
|
|
return;
|
|
|
|
if (offset < 0)
|
|
offset += length << 8; // try to make it positive
|
|
offset %= length;
|
|
if (offset < 0)
|
|
offset += length;
|
|
|
|
uint8_t *data = buf->data + start;
|
|
|
|
uint8_t *n_first = data + offset;
|
|
uint8_t *first = data;
|
|
uint8_t *next = n_first;
|
|
uint8_t *last = data + length;
|
|
|
|
while (first != next) {
|
|
uint8_t tmp = *first;
|
|
*first++ = *next;
|
|
*next++ = tmp;
|
|
if (next == last) {
|
|
next = n_first;
|
|
} else if (first == n_first) {
|
|
n_first = next;
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Write contents of `src` at `dstOffset` in current buffer.
|
|
*/
|
|
//%
|
|
void write(Buffer buf, int dstOffset, Buffer src) {
|
|
// srcOff and length not supported, we only do up to 4 args :/
|
|
writeBuffer(buf, dstOffset, src, 0, -1);
|
|
}
|
|
}
|
|
|
|
namespace control {
|
|
/**
|
|
* Create a new zero-initialized buffer.
|
|
* @param size number of bytes in the buffer
|
|
*/
|
|
//%
|
|
Buffer createBuffer(int size) {
|
|
return mkBuffer(NULL, size);
|
|
}
|
|
}
|
|
|
|
namespace pxt {
|
|
static int writeBytes(uint8_t *dst, uint8_t *src, int length, bool swapBytes, int szLeft) {
|
|
if (szLeft < length) {
|
|
return -1;
|
|
}
|
|
|
|
if (swapBytes) {
|
|
uint8_t *p = dst + length;
|
|
for (int i = 0; i < length; ++i)
|
|
*--p = src[i];
|
|
} else {
|
|
if (length == 4 && ((uint32_t)dst & 3) == 0)
|
|
*(uint32_t *)dst = *(uint32_t *)src;
|
|
else if (length == 2 && ((uint32_t)dst & 1) == 0)
|
|
*(uint16_t *)dst = *(uint16_t *)src;
|
|
else
|
|
memcpy(dst, src, length);
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int readBytes(uint8_t *src, uint8_t *dst, int length, bool swapBytes, int szLeft) {
|
|
if (szLeft < length) {
|
|
memset(dst, 0, length);
|
|
return -1;
|
|
}
|
|
|
|
if (swapBytes) {
|
|
uint8_t *p = src + length;
|
|
for (int i = 0; i < length; ++i)
|
|
dst[i] = *--p;
|
|
} else {
|
|
if (length == 4 && ((uint32_t)src & 3) == 0)
|
|
*(uint32_t *)dst = *(uint32_t *)src;
|
|
else if (length == 2 && ((uint32_t)src & 1) == 0)
|
|
*(uint16_t *)dst = *(uint16_t *)src;
|
|
else
|
|
memcpy(dst, src, length);
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
void setNumberCore(uint8_t *buf, int szLeft, NumberFormat format, TNumber value) {
|
|
int8_t i8;
|
|
uint8_t u8;
|
|
int16_t i16;
|
|
uint16_t u16;
|
|
int32_t i32;
|
|
uint32_t u32;
|
|
float f32;
|
|
double f64;
|
|
|
|
// Assume little endian
|
|
#define WRITEBYTES(isz, swap, toInt) \
|
|
isz = toInt(value); \
|
|
writeBytes(buf, (uint8_t *)&isz, sizeof(isz), swap, szLeft); \
|
|
break
|
|
|
|
switch (format) {
|
|
case NumberFormat::Int8LE:
|
|
WRITEBYTES(i8, false, toInt);
|
|
case NumberFormat::UInt8LE:
|
|
WRITEBYTES(u8, false, toInt);
|
|
case NumberFormat::Int16LE:
|
|
WRITEBYTES(i16, false, toInt);
|
|
case NumberFormat::UInt16LE:
|
|
WRITEBYTES(u16, false, toInt);
|
|
case NumberFormat::Int32LE:
|
|
WRITEBYTES(i32, false, toInt);
|
|
case NumberFormat::UInt32LE:
|
|
WRITEBYTES(u32, false, toUInt);
|
|
|
|
case NumberFormat::Int8BE:
|
|
WRITEBYTES(i8, true, toInt);
|
|
case NumberFormat::UInt8BE:
|
|
WRITEBYTES(u8, true, toInt);
|
|
case NumberFormat::Int16BE:
|
|
WRITEBYTES(i16, true, toInt);
|
|
case NumberFormat::UInt16BE:
|
|
WRITEBYTES(u16, true, toInt);
|
|
case NumberFormat::Int32BE:
|
|
WRITEBYTES(i32, true, toInt);
|
|
case NumberFormat::UInt32BE:
|
|
WRITEBYTES(u32, true, toUInt);
|
|
|
|
case NumberFormat::Float32LE:
|
|
WRITEBYTES(f32, false, toFloat);
|
|
case NumberFormat::Float32BE:
|
|
WRITEBYTES(f32, true, toFloat);
|
|
case NumberFormat::Float64LE:
|
|
WRITEBYTES(f64, false, toDouble);
|
|
case NumberFormat::Float64BE:
|
|
WRITEBYTES(f64, true, toDouble);
|
|
}
|
|
}
|
|
|
|
TNumber getNumberCore(uint8_t *buf, int szLeft, NumberFormat format) {
|
|
int8_t i8;
|
|
uint8_t u8;
|
|
int16_t i16;
|
|
uint16_t u16;
|
|
int32_t i32;
|
|
uint32_t u32;
|
|
float f32;
|
|
double f64;
|
|
|
|
// Assume little endian
|
|
#define READBYTES(isz, swap, conv) \
|
|
readBytes(buf, (uint8_t *)&isz, sizeof(isz), swap, szLeft); \
|
|
return conv(isz)
|
|
|
|
switch (format) {
|
|
case NumberFormat::Int8LE:
|
|
READBYTES(i8, false, fromInt);
|
|
case NumberFormat::UInt8LE:
|
|
READBYTES(u8, false, fromInt);
|
|
case NumberFormat::Int16LE:
|
|
READBYTES(i16, false, fromInt);
|
|
case NumberFormat::UInt16LE:
|
|
READBYTES(u16, false, fromInt);
|
|
case NumberFormat::Int32LE:
|
|
READBYTES(i32, false, fromInt);
|
|
case NumberFormat::UInt32LE:
|
|
READBYTES(u32, false, fromUInt);
|
|
|
|
case NumberFormat::Int8BE:
|
|
READBYTES(i8, true, fromInt);
|
|
case NumberFormat::UInt8BE:
|
|
READBYTES(u8, true, fromInt);
|
|
case NumberFormat::Int16BE:
|
|
READBYTES(i16, true, fromInt);
|
|
case NumberFormat::UInt16BE:
|
|
READBYTES(u16, true, fromInt);
|
|
case NumberFormat::Int32BE:
|
|
READBYTES(i32, true, fromInt);
|
|
case NumberFormat::UInt32BE:
|
|
READBYTES(u32, true, fromUInt);
|
|
|
|
case NumberFormat::Float32LE:
|
|
READBYTES(f32, false, fromFloat);
|
|
case NumberFormat::Float32BE:
|
|
READBYTES(f32, true, fromFloat);
|
|
case NumberFormat::Float64LE:
|
|
READBYTES(f64, false, fromDouble);
|
|
case NumberFormat::Float64BE:
|
|
READBYTES(f64, true, fromDouble);
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
} |