From 013523620963bf1a8275424d4e81e1e840bdf3ac Mon Sep 17 00:00:00 2001
From: Tau <tau@nowhere>
Date: Tue, 20 Nov 2018 20:21:52 -0500
Subject: [PATCH] board/sg-cmd.c: Add "sg" req/resp packets

---
 board/sg-cmd.c | 134 +++++++++++++++++++++++++++++++++++++++++++++++++
 board/sg-cmd.h |  43 ++++++++++++++++
 2 files changed, 177 insertions(+)
 create mode 100644 board/sg-cmd.c
 create mode 100644 board/sg-cmd.h

diff --git a/board/sg-cmd.c b/board/sg-cmd.c
new file mode 100644
index 0000000..a375686
--- /dev/null
+++ b/board/sg-cmd.c
@@ -0,0 +1,134 @@
+#include <assert.h>
+
+#include "board/sg-cmd.h"
+#include "board/sg-frame.h"
+
+#include "hook/iobuf.h"
+
+#include "util/dprintf.h"
+
+union sg_req_any {
+    struct sg_req_header req;
+    uint8_t bytes[256];
+};
+
+union sg_resp_any {
+    struct sg_resp_header resp;
+    uint8_t bytes[256];
+};
+
+static HRESULT sg_req_validate(const void *ptr, size_t nbytes);
+
+static void sg_resp_error(
+        struct sg_resp_header *resp,
+        const struct sg_req_header *req);
+
+static HRESULT sg_req_validate(const void *ptr, size_t nbytes)
+{
+    const struct sg_req_header *req;
+    size_t payload_len;
+
+    assert(ptr != NULL);
+
+    if (nbytes < sizeof(*req)) {
+        dprintf("SG Cmd: Request header truncated\n");
+
+        return E_FAIL;
+    }
+
+    req = ptr;
+
+    if (req->hdr.frame_len != nbytes) {
+        dprintf("SG Cmd: Frame length mismatch: got %i exp %i\n",
+                req->hdr.frame_len,
+                nbytes);
+
+        return E_FAIL;
+    }
+
+    payload_len = req->hdr.frame_len - sizeof(*req);
+
+    if (req->payload_len != payload_len) {
+        dprintf("SG Cmd: Payload length mismatch: got %i exp %i\n",
+                req->payload_len,
+                payload_len);
+
+        return E_FAIL;
+    }
+
+    return S_OK;
+}
+
+void sg_req_transact(
+        struct iobuf *resp_frame,
+        const uint8_t *req_bytes,
+        size_t req_nbytes,
+        sg_dispatch_fn_t dispatch,
+        void *ctx)
+{
+    struct iobuf req_span;
+    union sg_req_any req;
+    union sg_resp_any resp;
+    HRESULT hr;
+
+    assert(resp_frame != NULL);
+    assert(req_bytes != NULL);
+    assert(dispatch != NULL);
+
+    req_span.bytes = req.bytes;
+    req_span.nbytes = sizeof(req.bytes);
+    req_span.pos = 0;
+
+    hr = sg_frame_decode(&req_span, req_bytes, req_nbytes);
+
+    if (FAILED(hr)) {
+        return;
+    }
+
+    hr = sg_req_validate(req.bytes, req_span.pos);
+
+    if (FAILED(hr)) {
+        return;
+    }
+
+    hr = dispatch(ctx, &req, &resp);
+
+    if (hr != S_FALSE) {
+        if (FAILED(hr)) {
+            sg_resp_error(&resp.resp, &req.req);
+        }
+
+        sg_frame_encode(resp_frame, resp.bytes, resp.resp.hdr.frame_len);
+    }
+}
+
+void sg_resp_init(
+        struct sg_resp_header *resp,
+        const struct sg_req_header *req,
+        size_t payload_len)
+{
+    assert(resp != NULL);
+    assert(req != NULL);
+
+    resp->hdr.frame_len = sizeof(*resp) + payload_len;
+    resp->hdr.addr = req->hdr.addr;
+    resp->hdr.seq_no = req->hdr.seq_no;
+    resp->hdr.cmd = req->hdr.cmd;
+    resp->status = 0;
+    resp->payload_len = payload_len;
+}
+
+static void sg_resp_error(
+        struct sg_resp_header *resp,
+        const struct sg_req_header *req)
+{
+    assert(resp != NULL);
+    assert(req != NULL);
+
+    resp->hdr.frame_len = sizeof(*resp);
+    resp->hdr.addr = req->hdr.addr;
+    resp->hdr.seq_no = req->hdr.seq_no;
+    resp->hdr.cmd = req->hdr.cmd;
+    resp->status = 1;
+    resp->payload_len = 0;
+}
diff --git a/board/sg-cmd.h b/board/sg-cmd.h
new file mode 100644
index 0000000..ea2ae07
--- /dev/null
+++ b/board/sg-cmd.h
@@ -0,0 +1,43 @@
+#pragma once
+
+#include <windows.h>
+
+#include <stddef.h>
+#include <stdint.h>
+
+#include "hook/iobuf.h"
+
+struct sg_header {
+    uint8_t frame_len;
+    uint8_t addr;
+    uint8_t seq_no;
+    uint8_t cmd;
+};
+
+struct sg_req_header {
+    struct sg_header hdr;
+    uint8_t payload_len;
+};
+
+struct sg_resp_header {
+    struct sg_header hdr;
+    uint8_t status;
+    uint8_t payload_len;
+};
+
+typedef HRESULT (*sg_dispatch_fn_t)(
+        void *ctx,
+        const void *req,
+        void *resp);
+
+void sg_req_transact(
+        struct iobuf *resp_frame,
+        const uint8_t *req_bytes,
+        size_t req_nbytes,
+        sg_dispatch_fn_t dispatch,
+        void *ctx);
+
+void sg_resp_init(
+        struct sg_resp_header *resp,
+        const struct sg_req_header *req,
+        size_t payload_len);