106 lines
1.9 KiB
C
106 lines
1.9 KiB
C
#include <windows.h>
|
|
|
|
#include <stdbool.h>
|
|
#include <stdint.h>
|
|
|
|
#include "p3io/frame.h"
|
|
|
|
#include "util/iobuf.h"
|
|
#include "util/log.h"
|
|
|
|
HRESULT
|
|
p3io_frame_encode(struct iobuf *dest, const void *ptr, size_t nbytes)
|
|
{
|
|
const uint8_t *bytes;
|
|
uint8_t b;
|
|
size_t i;
|
|
|
|
log_assert(dest != NULL);
|
|
log_assert(ptr != NULL);
|
|
|
|
bytes = ptr;
|
|
|
|
if (dest->pos >= dest->nbytes) {
|
|
goto trunc;
|
|
}
|
|
|
|
dest->bytes[dest->pos++] = 0xAA;
|
|
|
|
for (i = 0; i < nbytes; i++) {
|
|
b = bytes[i];
|
|
|
|
if (b == 0xAA || b == 0xFF) {
|
|
if (dest->pos + 1 >= dest->nbytes) {
|
|
goto trunc;
|
|
}
|
|
|
|
dest->bytes[dest->pos++] = 0xFF;
|
|
dest->bytes[dest->pos++] = ~b;
|
|
} else {
|
|
if (dest->pos >= dest->nbytes) {
|
|
goto trunc;
|
|
}
|
|
|
|
dest->bytes[dest->pos++] = b;
|
|
}
|
|
}
|
|
|
|
if (i < nbytes) {
|
|
goto trunc;
|
|
}
|
|
|
|
return S_OK;
|
|
|
|
trunc:
|
|
return E_FAIL;
|
|
}
|
|
|
|
HRESULT
|
|
p3io_frame_decode(struct iobuf *dest, struct const_iobuf *src)
|
|
{
|
|
bool escape;
|
|
uint8_t b;
|
|
|
|
log_assert(dest != NULL);
|
|
log_assert(src != NULL);
|
|
|
|
if (src->pos >= src->nbytes || src->bytes[src->pos] != 0xAA) {
|
|
return E_FAIL;
|
|
}
|
|
|
|
src->pos++;
|
|
escape = false;
|
|
|
|
while (src->pos < src->nbytes) {
|
|
if (dest->pos >= dest->nbytes) {
|
|
return E_FAIL;
|
|
}
|
|
|
|
b = src->bytes[src->pos++];
|
|
|
|
if (b == 0xAA) {
|
|
return E_FAIL;
|
|
} else if (b == 0xFF) {
|
|
if (escape) {
|
|
return E_FAIL;
|
|
}
|
|
|
|
escape = true;
|
|
} else {
|
|
if (escape) {
|
|
dest->bytes[dest->pos++] = ~b;
|
|
} else {
|
|
dest->bytes[dest->pos++] = b;
|
|
}
|
|
|
|
escape = false;
|
|
}
|
|
}
|
|
|
|
if (escape) {
|
|
return E_FAIL;
|
|
}
|
|
|
|
return S_OK;
|
|
}
|