X-Git-Url: https://git.stg.codes/stg.git/blobdiff_plain/b27841d687ec9e84983340b5581376dfb24010ea..b2b89723a2427bba8290bd6967a1ab39cbb630be:/libs/smux/oer_support.c

diff --git a/libs/smux/oer_support.c b/libs/smux/oer_support.c
new file mode 100644
index 00000000..b15a3bc9
--- /dev/null
+++ b/libs/smux/oer_support.c
@@ -0,0 +1,122 @@
+/*
+ * Copyright (c) 2017 Lev Walkin <vlm@lionet.info>.
+ * All rights reserved.
+ * Redistribution and modifications are permitted subject to BSD license.
+ */
+#include <asn_system.h>
+#include <asn_internal.h>
+
+#include <oer_support.h>
+
+/*
+ * Fetch the length determinant (X.696 08/2015, #8.6) into *len_r.
+ * RETURN VALUES:
+ *       0:     More data expected than bufptr contains.
+ *      -1:     Fatal error deciphering length.
+ *      >0:     Number of bytes used from bufptr.
+ */
+ssize_t
+oer_fetch_length(const void *bufptr, size_t size, size_t *len_r) {
+    uint8_t first_byte;
+    size_t len_len;    /* Length of the length determinant */
+    const uint8_t *b;
+    const uint8_t *bend;
+    size_t len;
+
+    if(size == 0) {
+        *len_r = 0;
+        return 0;
+    }
+
+    first_byte = *(const uint8_t *)bufptr;
+    if((first_byte & 0x80) == 0) {   /* Short form */
+        *len_r = first_byte; /* 0..127 */
+        return 1;
+    }
+
+    len_len = (first_byte & 0x7f);
+    if((1 + len_len) > size) {
+        *len_r = 0;
+        return 0;
+    }
+
+    b = (const uint8_t *)bufptr + 1;
+    bend = b + len_len;
+
+    for(; b < bend && *b == 0; b++) {
+        /* Skip the leading 0-bytes */
+    }
+
+    if((bend - b) > (ssize_t)sizeof(size_t)) {
+        /* Length is not representable by the native size_t type */
+        *len_r = 0;
+        return -1;
+    }
+
+    for(len = 0; b < bend; b++) {
+        len = (len << 8) + *b;
+    }
+
+    if(len > RSIZE_MAX) { /* A bit of C11 validation */
+        *len_r = 0;
+        return -1;
+    }
+
+    *len_r = len;
+    assert(len_len + 1 == (size_t)(bend - (const uint8_t *)bufptr));
+    return len_len + 1;
+}
+
+
+/*
+ * Serialize OER length. Returns the number of bytes serialized
+ * or -1 if a given callback returned with negative result.
+ */
+ssize_t
+oer_serialize_length(size_t length, asn_app_consume_bytes_f *cb,
+                     void *app_key) {
+    uint8_t scratch[1 + sizeof(length)];
+    uint8_t *sp = scratch;
+    int littleEndian = 1;   /* Run-time detection */
+    const uint8_t *pstart;
+    const uint8_t *pend;
+    const uint8_t *p;
+    int add;
+
+    if(length <= 127) {
+        uint8_t b = length;
+        if(cb(&b, 1, app_key) < 0) {
+            return -1;
+        }
+        return 1;
+    }
+
+    if(*(char *)&littleEndian) {
+        pstart = (const uint8_t *)&length + sizeof(length) - 1;
+        pend = (const uint8_t *)&length;
+        add = -1;
+    } else {
+        pstart = (const uint8_t *)&length;
+        pend = pstart + sizeof(length);
+        add = 1;
+    }
+
+    for(p = pstart; p != pend; p += add) {
+        /* Skip leading zeros. */
+        if(*p) break;
+    }
+
+    for(sp = scratch + 1; ; p += add) {
+        *sp++ = *p;
+        if(p == pend) break;
+    }
+    assert((sp - scratch) - 1 <= 0x7f);
+    scratch[0] = 0x80 + ((sp - scratch) - 1);
+
+    if(cb(scratch, sp - scratch, app_key) < 0) {
+        return -1;
+    }
+
+    return sp - scratch;
+}
+