segatools/chunihook/slider-frame.c

181 lines
4.0 KiB
C
Raw Normal View History

#include <windows.h>
2018-11-23 19:37:42 +00:00
#include <assert.h>
#include <stdbool.h>
#include <stddef.h>
#include <stdint.h>
#include "chunihook/slider-frame.h"
#include "hook/iobuf.h"
static void slider_frame_sync(struct iobuf *src);
static HRESULT slider_frame_accept(const struct iobuf *dest);
static HRESULT slider_frame_encode_byte(struct iobuf *dest, uint8_t byte);
/* Frame structure:
[0] Sync byte (0xFF)
[1] Command
[2] Payload size
... Payload
[n] Checksum: Negate the sum of all prior bytes (including sync byte!)
Byte stuffing:
0xFD is an escape byte. Un-escape the subsequent byte by adding 1. */
static void slider_frame_sync(struct iobuf *src)
{
size_t i;
for (i = 0 ; i < src->pos && src->bytes[i] != 0xFF ; i++);
src->pos -= i;
memmove(&src->bytes[0], &src->bytes[i], i);
}
static HRESULT slider_frame_accept(const struct iobuf *dest)
{
uint8_t checksum;
size_t i;
if (dest->pos < 2 || dest->pos != dest->bytes[2] + 4) {
return S_FALSE;
}
checksum = 0;
for (i = 0 ; i < dest->pos ; i++) {
checksum += dest->bytes[i];
}
if (checksum != 0) {
return HRESULT_FROM_WIN32(ERROR_CRC);
}
return S_OK;
}
HRESULT slider_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);
slider_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 == 0xFF) {
hr = E_FAIL;
} else if (byte == 0xFD) {
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 = slider_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 slider_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 >= 2 && src[0] == 0xFF && src[2] + 3 == nbytes);
if (dest->pos >= dest->nbytes) {
return HRESULT_FROM_WIN32(ERROR_INSUFFICIENT_BUFFER);
}
dest->bytes[dest->pos++] = 0xFF;
checksum = 0xFF;
for (i = 1 ; i < nbytes ; i++) {
byte = src[i];
checksum += byte;
hr = slider_frame_encode_byte(dest, byte);
if (FAILED(hr)) {
return hr;
}
}
return slider_frame_encode_byte(dest, -checksum);
}
static HRESULT slider_frame_encode_byte(struct iobuf *dest, uint8_t byte)
{
if (byte == 0xFF || byte == 0xFD) {
if (dest->pos + 2 > dest->nbytes) {
return HRESULT_FROM_WIN32(ERROR_INSUFFICIENT_BUFFER);
}
dest->bytes[dest->pos++] = 0xFD;
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;
}