Bemanitools v5.26 release

src/main/p3io/frame.c

@@ -0,0 +1,108 @@
+#include <windows.h>
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "p3io/frame.h"
+
+#include "util/iobuf.h"
+#include "util/log.h"
+
+HRESULT p3io_frame_encode(
+        struct iobuf *dest,
+        const void *ptr,
+        size_t nbytes)
+{
+    const uint8_t *bytes;
+    uint8_t b;
+    size_t i;
+
+    log_assert(dest != NULL);
+    log_assert(ptr != NULL);
+
+    bytes = ptr;
+
+    if (dest->pos >= dest->nbytes) {
+        goto trunc;
+    }
+
+    dest->bytes[dest->pos++] = 0xAA;
+
+    for (i = 0 ; i < nbytes ; i++) {
+        b = bytes[i];
+
+        if (b == 0xAA || b == 0xFF) {
+            if (dest->pos + 1 >= dest->nbytes) {
+                goto trunc;
+            }
+
+            dest->bytes[dest->pos++] = 0xFF;
+            dest->bytes[dest->pos++] = ~b;
+        } else {
+            if (dest->pos >= dest->nbytes) {
+                goto trunc;
+            }
+
+            dest->bytes[dest->pos++] = b;
+        }
+    }
+
+    if (i < nbytes) {
+        goto trunc;
+    }
+
+    return S_OK;
+
+trunc:
+    return E_FAIL;
+}
+
+HRESULT p3io_frame_decode(
+        struct iobuf *dest,
+        struct const_iobuf *src)
+{
+    bool escape;
+    uint8_t b;
+
+    log_assert(dest != NULL);
+    log_assert(src != NULL);
+
+    if (src->pos >= src->nbytes || src->bytes[src->pos] != 0xAA) {
+        return E_FAIL;
+    }
+
+    src->pos++;
+    escape = false;
+
+    while (src->pos < src->nbytes) {
+        if (dest->pos >= dest->nbytes) {
+            return E_FAIL;
+        }
+
+        b = src->bytes[src->pos++];
+
+        if (b == 0xAA) {
+            return E_FAIL;
+        } else if (b == 0xFF) {
+            if (escape) {
+                return E_FAIL;
+            }
+
+            escape = true;
+        } else {
+            if (escape) {
+                dest->bytes[dest->pos++] = ~b;
+            } else {
+                dest->bytes[dest->pos++] = b;
+            }
+
+            escape = false;
+        }
+    }
+
+    if (escape) {
+        return E_FAIL;
+    }
+
+    return S_OK;
+}