166 lines
3.4 KiB
C
166 lines
3.4 KiB
C
#include <windows.h>
|
|
|
|
#include <assert.h>
|
|
#include <stdbool.h>
|
|
#include <stddef.h>
|
|
#include <stdint.h>
|
|
|
|
#include "board/sg-frame.h"
|
|
|
|
#include "hook/iobuf.h"
|
|
|
|
#include "util/dprintf.h"
|
|
|
|
static HRESULT sg_frame_accept(struct iobuf *dest);
|
|
static HRESULT sg_frame_encode_byte(struct iobuf *dest, uint8_t byte);
|
|
|
|
/* Frame structure:
|
|
|
|
[0] Sync byte (0xE0)
|
|
[1] Frame size (including self)
|
|
[2] Address
|
|
[3] Sequence no
|
|
... Body
|
|
[n] Checksum: Sum of all non-framing bytes
|
|
|
|
Byte stuffing:
|
|
|
|
0xD0 is an escape byte. Un-escape the subsequent byte by adding 1. */
|
|
|
|
static HRESULT sg_frame_accept(struct iobuf *dest)
|
|
{
|
|
uint8_t checksum;
|
|
size_t i;
|
|
|
|
if (dest->pos < 1 || dest->pos != dest->bytes[0] + 1) {
|
|
dprintf("SG Frame: Size mismatch\n");
|
|
|
|
return S_FALSE;
|
|
}
|
|
|
|
checksum = 0;
|
|
|
|
for (i = 0 ; i < dest->pos - 1 ; i++) {
|
|
checksum += dest->bytes[i];
|
|
}
|
|
|
|
if (checksum != dest->bytes[dest->pos - 1]) {
|
|
dprintf("SG Frame: Checksum mismatch\n");
|
|
|
|
return HRESULT_FROM_WIN32(ERROR_CRC);
|
|
}
|
|
|
|
/* Discard checksum */
|
|
dest->pos--;
|
|
|
|
return S_OK;
|
|
}
|
|
|
|
HRESULT sg_frame_decode(struct iobuf *dest, const uint8_t *bytes, size_t nbytes)
|
|
{
|
|
uint8_t byte;
|
|
size_t i;
|
|
|
|
assert(dest != NULL);
|
|
assert(dest->bytes != NULL || dest->nbytes == 0);
|
|
assert(dest->pos <= dest->nbytes);
|
|
assert(bytes != NULL);
|
|
|
|
if (nbytes < 1 || bytes[0] != 0xE0) {
|
|
dprintf("SG Frame: Bad sync\n");
|
|
|
|
return E_FAIL;
|
|
}
|
|
|
|
dest->pos = 0;
|
|
i = 1;
|
|
|
|
while (i < nbytes) {
|
|
if (dest->pos >= dest->nbytes) {
|
|
return HRESULT_FROM_WIN32(ERROR_INSUFFICIENT_BUFFER);
|
|
}
|
|
|
|
byte = bytes[i++];
|
|
|
|
if (byte == 0xE0) {
|
|
dprintf("SG Frame: Unescaped sync\n");
|
|
|
|
return E_FAIL;
|
|
} else if (byte == 0xD0) {
|
|
if (i >= nbytes) {
|
|
dprintf("SG Frame: Trailing escape\n");
|
|
|
|
return E_FAIL;
|
|
}
|
|
|
|
byte = bytes[i++];
|
|
dest->bytes[dest->pos++] = byte + 1;
|
|
} else {
|
|
dest->bytes[dest->pos++] = byte;
|
|
}
|
|
}
|
|
|
|
return sg_frame_accept(dest);
|
|
}
|
|
|
|
HRESULT sg_frame_encode(
|
|
struct iobuf *dest,
|
|
const void *ptr,
|
|
size_t nbytes)
|
|
{
|
|
const uint8_t *src;
|
|
uint8_t checksum;
|
|
uint8_t byte;
|
|
size_t i;
|
|
HRESULT hr;
|
|
|
|
assert(dest != NULL);
|
|
assert(dest->bytes != NULL || dest->nbytes == 0);
|
|
assert(dest->pos <= dest->nbytes);
|
|
assert(ptr != NULL);
|
|
|
|
src = ptr;
|
|
|
|
assert(nbytes != 0 && src[0] == nbytes);
|
|
|
|
if (dest->pos >= dest->nbytes) {
|
|
return HRESULT_FROM_WIN32(ERROR_INSUFFICIENT_BUFFER);
|
|
}
|
|
|
|
dest->bytes[dest->pos++] = 0xE0;
|
|
checksum = 0;
|
|
|
|
for (i = 0 ; i < nbytes ; i++) {
|
|
byte = src[i];
|
|
checksum += byte;
|
|
|
|
hr = sg_frame_encode_byte(dest, byte);
|
|
|
|
if (FAILED(hr)) {
|
|
return hr;
|
|
}
|
|
}
|
|
|
|
return sg_frame_encode_byte(dest, checksum);
|
|
}
|
|
|
|
static HRESULT sg_frame_encode_byte(struct iobuf *dest, uint8_t byte)
|
|
{
|
|
if (byte == 0xD0 || byte == 0xE0) {
|
|
if (dest->pos + 2 > dest->nbytes) {
|
|
return HRESULT_FROM_WIN32(ERROR_INSUFFICIENT_BUFFER);
|
|
}
|
|
|
|
dest->bytes[dest->pos++] = 0xD0;
|
|
dest->bytes[dest->pos++] = byte - 1;
|
|
} else {
|
|
if (dest->pos + 1 > dest->nbytes) {
|
|
return HRESULT_FROM_WIN32(ERROR_INSUFFICIENT_BUFFER);
|
|
}
|
|
|
|
dest->bytes[dest->pos++] = byte;
|
|
}
|
|
|
|
return S_OK;
|
|
}
|