From 484931ccf39ac0323477d8716bb271228aa8da1b Mon Sep 17 00:00:00 2001
From: Tau <tau@nowhere>
Date: Thu, 8 Nov 2018 14:18:34 -0500
Subject: [PATCH] jvs/jvs-frame.c: Add JVS framing

---
 jvs/jvs-frame.c | 151 ++++++++++++++++++++++++++++++++++++++++++++++++
 jvs/jvs-frame.h |  17 ++++++
 jvs/meson.build |   2 +
 3 files changed, 170 insertions(+)
 create mode 100644 jvs/jvs-frame.c
 create mode 100644 jvs/jvs-frame.h

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 <windows.h>
+
+#include <assert.h>
+#include <stddef.h>
+#include <stdint.h>
+
+#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 <windows.h>
+
+#include <stddef.h>
+
+#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',
     ],
 )