73 lines
2.3 KiB
C
73 lines
2.3 KiB
C
|
|
inline int cobs_encode(uint8_t *buf_in, unsigned int buf_in_size, uint8_t *buf_out,
|
|
unsigned int buf_out_size) {
|
|
unsigned int i_out = 1;
|
|
unsigned int last0 = 0;
|
|
for (unsigned int i_in = 0; i_in < buf_in_size; i_in += 1, i_out += 1) {
|
|
if (i_out >= buf_out_size) return OUT_OF_MEMORY;
|
|
if ((buf_in[i_in] != 0x00) && ((i_out - last0) < 0xFF)) {
|
|
buf_out[i_out] = buf_in[i_in];
|
|
} else {
|
|
buf_out[last0] = i_out - last0;
|
|
last0 = i_out;
|
|
}
|
|
}
|
|
buf_out[last0] = i_out - last0;
|
|
buf_out[i_out] = 0x00;
|
|
return i_out + 1;
|
|
}
|
|
|
|
inline int cobs_decode(uint8_t *buf_in, unsigned int buf_in_size, uint8_t *buf_out,
|
|
unsigned int buf_out_size) {
|
|
unsigned int i_out = 0;
|
|
unsigned int next0 = 0;
|
|
bool skip_next0 = true;
|
|
for (unsigned int i_in = 0; i_in < buf_in_size || buf_in[i_in] == 0x00; i_in += 1) {
|
|
if (i_out >= buf_out_size) return OUT_OF_MEMORY;
|
|
if (next0 == i_in) {
|
|
if (!skip_next0) buf_out[i_out++] = 0x00;
|
|
next0 = buf_in[i_in];
|
|
skip_next0 = (next0 - i_in) == 0xFF;
|
|
} else
|
|
buf_out[i_out++] = buf_in[i_in];
|
|
}
|
|
return i_out-1;
|
|
}
|
|
|
|
inline unsigned int encode_zigzag(int input) {
|
|
return (input << 1) ^ (input >> (sizeof(int) * 8 - 1));
|
|
}
|
|
|
|
inline unsigned int decode_zigzag(int input) { return (input >> 1) ^ (-(input & 1)); }
|
|
|
|
inline unsigned int encode_unsigned_varint(unsigned int input, uint8_t *out) {
|
|
int offset = 0;
|
|
while (input > 0x7F) {
|
|
out[offset++] = input & 0x7F;
|
|
input >>= 7;
|
|
}
|
|
out[offset++] = input | 0x80;
|
|
return offset;
|
|
}
|
|
|
|
inline unsigned int encode_varint(int input, uint8_t *out) {
|
|
return encode_unsigned_varint(encode_zigzag(input), out);
|
|
}
|
|
|
|
inline unsigned int decode_unsigned_varint(uint8_t *input, unsigned int &out) {
|
|
int offset = 0;
|
|
out = 0;
|
|
for (;; offset += 1) {
|
|
out |= ((unsigned int)(input[offset] & 0x7F)) << (7 * offset);
|
|
if (input[offset] & 0x80) break;
|
|
}
|
|
return offset + 1;
|
|
}
|
|
|
|
inline unsigned int decode_varint(uint8_t *input, int &out) {
|
|
unsigned int out_uint;
|
|
int offset = decode_unsigned_varint(input, out_uint);
|
|
out = decode_zigzag(out_uint);
|
|
return offset;
|
|
}
|