From b8ffd20877fbfc42d7a7d02f9dbc4d4e289e01f5 Mon Sep 17 00:00:00 2001
From: Tau <tau@nowhere>
Date: Tue, 20 Nov 2018 20:21:23 -0500
Subject: [PATCH] board/sg-frame.c: Add "sg" framing support

---
 board/meson.build |   2 +
 board/sg-frame.c  | 164 ++++++++++++++++++++++++++++++++++++++++++++++
 board/sg-frame.h  |  15 +++++
 3 files changed, 181 insertions(+)
 create mode 100644 board/sg-frame.c
 create mode 100644 board/sg-frame.h

diff --git a/board/meson.build b/board/meson.build
index 6bde450..ed763ff 100644
--- a/board/meson.build
+++ b/board/meson.build
@@ -9,5 +9,7 @@ board_lib = static_library(
     sources : [
         'io3.c',
         'io3.h',
+        'sg-frame.c',
+        'sg-frame.h',
     ],
 )
diff --git a/board/sg-frame.c b/board/sg-frame.c
new file mode 100644
index 0000000..d199af5
--- /dev/null
+++ b/board/sg-frame.c
@@ -0,0 +1,164 @@
+#include <windows.h>
+
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdint.h>
+
+#include "board/sg-frame.h"
+
+#include "hook/iobuf.h"
+
+#include "util/dprintf.h"
+
+static HRESULT sg_frame_accept(struct iobuf *dest);
+static HRESULT sg_frame_encode_byte(struct iobuf *dest, uint8_t byte);
+
+/* Frame structure:
+
+   [0] Sync byte (0xE0)
+   [1] Frame size (including self)
+   [2] Address
+   [3] Sequence no
+   ... Body
+   [n] Checksum: Sum of all non-framing bytes
+
+   Byte stuffing:
+
+   0xD0 is an escape byte. Un-escape the subsequent byte by adding 1. */
+
+static HRESULT sg_frame_accept(struct iobuf *dest)
+{
+    uint8_t checksum;
+    size_t i;
+
+    if (dest->pos < 1 || dest->pos != dest->bytes[0] + 1) {
+        dprintf("SG Frame: Size mismatch\n");
+
+        return S_FALSE;
+    }
+
+    checksum = 0;
+
+    for (i = 0 ; i < dest->pos - 1 ; i++) {
+        checksum += dest->bytes[i];
+    }
+
+    if (checksum != dest->bytes[dest->pos - 1]) {
+        dprintf("SG Frame: Checksum mismatch\n");
+
+        return HRESULT_FROM_WIN32(ERROR_CRC);
+    }
+
+    /* Discard checksum */
+    dest->pos--;
+
+    return S_OK;
+}
+
+HRESULT sg_frame_decode(struct iobuf *dest, const uint8_t *bytes, size_t nbytes)
+{
+    uint8_t byte;
+    size_t i;
+
+    assert(dest != NULL);
+    assert(dest->bytes != NULL || dest->nbytes == 0);
+    assert(dest->pos <= dest->nbytes);
+    assert(bytes != NULL);
+
+    if (nbytes < 1 || bytes[0] != 0xE0) {
+        dprintf("SG Frame: Bad sync\n");
+
+        return E_FAIL;
+    }
+
+    dest->pos = 0;
+    i = 1;
+
+    while (i < nbytes) {
+        if (dest->pos >= dest->nbytes) {
+            return HRESULT_FROM_WIN32(ERROR_INSUFFICIENT_BUFFER);
+        }
+
+        byte = bytes[i++];
+
+        if (byte == 0xE0) {
+            dprintf("SG Frame: Unescaped sync\n");
+
+            return E_FAIL;
+        } else if (byte == 0xD0) {
+            if (i >= nbytes) {
+                dprintf("SG Frame: Trailing escape\n");
+
+                return E_FAIL;
+            }
+
+            byte = bytes[i++];
+            dest->bytes[dest->pos++] = byte + 1;
+        } else {
+            dest->bytes[dest->pos++] = byte;
+        }
+    }
+
+    return sg_frame_accept(dest);
+}
+
+HRESULT sg_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 != 0 && src[0] == nbytes);
+
+    if (dest->pos >= dest->nbytes) {
+        return HRESULT_FROM_WIN32(ERROR_INSUFFICIENT_BUFFER);
+    }
+
+    dest->bytes[dest->pos++] = 0xE0;
+    checksum = 0;
+
+    for (i = 0 ; i < nbytes ; i++) {
+        byte = src[i];
+        checksum += byte;
+
+        hr = sg_frame_encode_byte(dest, byte);
+
+        if (FAILED(hr)) {
+            return hr;
+        }
+    }
+
+    return sg_frame_encode_byte(dest, checksum);
+}
+
+static HRESULT sg_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/board/sg-frame.h b/board/sg-frame.h
new file mode 100644
index 0000000..d74d9b3
--- /dev/null
+++ b/board/sg-frame.h
@@ -0,0 +1,15 @@
+#pragma once
+
+#include <windows.h>
+
+#include <stddef.h>
+#include <stdint.h>
+
+#include "hook/iobuf.h"
+
+HRESULT sg_frame_decode(
+        struct iobuf *dest,
+        const uint8_t *bytes,
+        size_t nbytes);
+
+HRESULT sg_frame_encode(struct iobuf *dest, const void *ptr, size_t nbytes);