diff --git a/board/meson.build b/board/meson.build index 6bde4509..0c515fce 100644 --- a/board/meson.build +++ b/board/meson.build @@ -9,5 +9,7 @@ board_lib = static_library( sources : [ 'io3.c', 'io3.h', + 'slider-frame.c', + 'slider-frame.h', ], ) diff --git a/board/slider-frame.c b/board/slider-frame.c new file mode 100644 index 00000000..c1409e8b --- /dev/null +++ b/board/slider-frame.c @@ -0,0 +1,179 @@ +#include + +#include +#include +#include + +#include "board/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; +} diff --git a/board/slider-frame.h b/board/slider-frame.h new file mode 100644 index 00000000..af037f0e --- /dev/null +++ b/board/slider-frame.h @@ -0,0 +1,25 @@ +#pragma once + +#include + +#include +#include + +#include "hook/iobuf.h" + +enum { + SLIDER_FRAME_SYNC = 0xFF, +}; + +struct slider_hdr { + uint8_t sync; + uint8_t cmd; + uint8_t nbytes; +}; + +HRESULT slider_frame_decode(struct iobuf *dest, struct iobuf *src); + +HRESULT slider_frame_encode( + struct iobuf *dest, + const void *ptr, + size_t nbytes);