diff --git a/jvs/jvs-frame.c b/jvs/jvs-frame.c new file mode 100644 index 0000000..acfff05 --- /dev/null +++ b/jvs/jvs-frame.c @@ -0,0 +1,151 @@ +#include + +#include +#include +#include + +#include "hook/iobuf.h" + +#include "jvs/jvs-frame.h" + +#include "util/dprintf.h" + +static HRESULT jvs_frame_check_sum(const struct iobuf *dest); +static HRESULT jvs_frame_encode_byte(struct iobuf *dest, uint8_t byte); + +/* Deals in whole frames only for simplicity's sake, since that's all we need + to emulate the Nu's kernel driver interface. This could of course be + extended later for other arcade hardware platforms. */ + +HRESULT jvs_frame_decode( + struct iobuf *dest, + const void *ptr, + size_t nbytes) +{ + const uint8_t *bytes; + bool escape; + size_t i; + + assert(dest != NULL); + assert(ptr != NULL); + + bytes = ptr; + + if (nbytes == 0) { + dprintf("JVS Frame: Empty frame\n"); + + return E_FAIL; + } + + if (bytes[0] != 0xE0) { + dprintf("JVS Frame: Sync byte was expected\n"); + + return E_FAIL; + } + + escape = false; + + for (i = 1 ; i < nbytes ; i++) { + if (bytes[i] == 0xE0) { + dprintf("JVS Frame: Unexpected sync byte\n"); + + return E_FAIL; + } else if (bytes[i] == 0xD0) { + if (escape) { + dprintf("JVS Frame: Escaping fault\n"); + + return E_FAIL; + } + + escape = true; + } else { + if (dest->pos >= dest->nbytes) { + return HRESULT_FROM_WIN32(ERROR_INSUFFICIENT_BUFFER); + } + + if (escape) { + escape = false; + dest->bytes[dest->pos++] = bytes[i] + 1; + } else { + dest->bytes[dest->pos++] = bytes[i]; + } + } + } + + return jvs_frame_check_sum(dest); +} + +static HRESULT jvs_frame_check_sum(const struct iobuf *dest) +{ + uint8_t checksum; + size_t i; + + checksum = 0; + + for (i = 0 ; i < dest->pos - 1 ; i++) { + checksum += dest->bytes[i]; + } + + if (checksum != dest->bytes[dest->pos - 1]) { + dprintf("JVS Frame: Checksum failure\n"); + + return HRESULT_FROM_WIN32(ERROR_CRC); + } + + return S_OK; +} + +HRESULT jvs_frame_encode( + struct iobuf *dest, + const void *ptr, + size_t nbytes) +{ + const uint8_t *bytes; + uint8_t checksum; + size_t i; + HRESULT hr; + + assert(dest != NULL); + assert(ptr != NULL); + + bytes = ptr; + checksum = 0; + + if (dest->pos + 1 > dest->nbytes) { + return HRESULT_FROM_WIN32(ERROR_INSUFFICIENT_BUFFER); + } + + dest->bytes[dest->pos++] = 0xE0; + + for (i = 0 ; i < nbytes ; i++) { + hr = jvs_frame_encode_byte(dest, bytes[i]); + + if (FAILED(hr)) { + return hr; + } + + checksum += bytes[i]; + } + + return jvs_frame_encode_byte(dest, checksum); +} + +static HRESULT jvs_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; +} diff --git a/jvs/jvs-frame.h b/jvs/jvs-frame.h new file mode 100644 index 0000000..719e528 --- /dev/null +++ b/jvs/jvs-frame.h @@ -0,0 +1,17 @@ +#pragma once + +#include + +#include + +#include "hook/iobuf.h" + +HRESULT jvs_frame_decode( + struct iobuf *dest, + const void *bytes, + size_t nbytes); + +HRESULT jvs_frame_encode( + struct iobuf *dest, + const void *bytes, + size_t nbytes); diff --git a/jvs/meson.build b/jvs/meson.build index f39045a..b08aba3 100644 --- a/jvs/meson.build +++ b/jvs/meson.build @@ -9,5 +9,7 @@ jvs_lib = static_library( sources : [ 'jvs-bus.c', 'jvs-bus.h', + 'jvs-frame.c', + 'jvs-frame.h', ], )