segatools/board/led15093-frame.c

197 lines
4.7 KiB
C

#include <windows.h>
#include <assert.h>
#include <stdbool.h>
#include <stddef.h>
#include <stdint.h>
#include "board/led15093-frame.h"
#include "hook/iobuf.h"
static void led15093_frame_sync(struct iobuf *src);
static HRESULT led15093_frame_accept(const struct iobuf *dest);
static HRESULT led15093_frame_encode_byte(struct iobuf *dest, uint8_t byte);
/* Frame structure:
[0] Sync byte (0xE0)
[1] Destination address
[2] Source Address
[3] Length of data/payload
[4] Data/payload
For requests (host to board):
[0] Command
... Payload
For responses (board to host):
[0] Status
[1] Command
[2] Report
... Payload
[n] Checksum: Sum of all prior bytes (excluding sync byte)
Byte stuffing:
0xD0 is an escape byte. Un-escape the subsequent byte by adding 1. */
static void led15093_frame_sync(struct iobuf *src)
{
size_t i;
for (i = 0 ; i < src->pos && src->bytes[i] != LED_15093_FRAME_SYNC ; i++);
src->pos -= i;
memmove(&src->bytes[0], &src->bytes[i], i);
}
static HRESULT led15093_frame_accept(const struct iobuf *dest)
{
uint8_t checksum;
size_t i;
if (dest->pos < 3 || dest->pos != dest->bytes[3] + 5) {
return S_FALSE;
}
checksum = 0;
for (i = 1 ; i < dest->pos - 1 ; i++) {
checksum += dest->bytes[i];
}
// dprintf("LED checksum %02x, expected %02x\n", checksum, dest->bytes[dest->pos - 1]);
if (checksum != dest->bytes[dest->pos - 1]) {
return HRESULT_FROM_WIN32(ERROR_CRC);
}
return S_OK;
}
HRESULT led15093_frame_decode(struct iobuf *dest, struct iobuf *src)
{
uint8_t byte;
bool escape;
size_t i;
HRESULT hr;
assert(dest != NULL);
assert(dest->bytes != NULL || dest->nbytes == 0);
assert(dest->pos <= dest->nbytes);
assert(src != NULL);
assert(src->bytes != NULL || src->nbytes == 0);
assert(src->pos <= src->nbytes);
led15093_frame_sync(src);
dest->pos = 0;
escape = false;
for (i = 0, hr = S_FALSE ; i < src->pos && hr == S_FALSE ; i++) {
/* Step the FSM to unstuff another byte */
byte = src->bytes[i];
if (dest->pos >= dest->nbytes) {
hr = HRESULT_FROM_WIN32(ERROR_INSUFFICIENT_BUFFER);
} else if (i == 0) {
dest->bytes[dest->pos++] = byte;
} else if (byte == LED_15093_FRAME_SYNC) {
hr = E_FAIL;
} else if (byte == LED_15093_FRAME_ESC) {
if (escape) {
hr = E_FAIL;
}
escape = true;
} else if (escape) {
dest->bytes[dest->pos++] = byte + 1;
escape = false;
} else {
dest->bytes[dest->pos++] = byte;
}
/* Try to accept the packet we've built up so far */
if (SUCCEEDED(hr)) {
hr = led15093_frame_accept(dest);
}
}
/* Handle FSM terminal state */
if (hr != S_FALSE) {
/* Frame was either accepted or rejected, remove it from src */
memmove(&src->bytes[0], &src->bytes[i], src->pos - i);
src->pos -= i;
}
return hr;
}
HRESULT led15093_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 >= 3 &&
src[0] == LED_15093_FRAME_SYNC &&
src[3] + 4 == nbytes);
if (dest->pos >= dest->nbytes) {
return HRESULT_FROM_WIN32(ERROR_INSUFFICIENT_BUFFER);
}
dest->bytes[dest->pos++] = LED_15093_FRAME_SYNC;
checksum = 0;
// dprintf("%02x ", LED_15093_FRAME_SYNC);
for (i = 1 ; i < nbytes ; i++) {
byte = src[i];
checksum += byte;
// dprintf("%02x ", byte);
hr = led15093_frame_encode_byte(dest, byte);
if (FAILED(hr)) {
return hr;
}
}
// dprintf("%02x \n", checksum);
return led15093_frame_encode_byte(dest, checksum);
}
static HRESULT led15093_frame_encode_byte(struct iobuf *dest, uint8_t byte)
{
if (byte == LED_15093_FRAME_SYNC || byte == LED_15093_FRAME_ESC) {
if (dest->pos + 2 > dest->nbytes) {
return HRESULT_FROM_WIN32(ERROR_INSUFFICIENT_BUFFER);
}
dest->bytes[dest->pos++] = LED_15093_FRAME_ESC;
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;
}