1 files changed, 107 insertions, 0 deletions
diff --git a/lib/erl_interface/src/encode/encode_binary.c b/lib/erl_interface/src/encode/encode_binary.c
index 4471c51769..0562979417 100644
--- a/lib/erl_interface/src/encode/encode_binary.c
+++ b/lib/erl_interface/src/encode/encode_binary.c
@@ -22,6 +22,10 @@
 #include "eiext.h"
 #include "putget.h"
 
+static void copy_bits(const unsigned char* src, size_t soffs,
+                      unsigned char* dst, size_t n);
+
+
 int ei_encode_binary(char *buf, int *index, const void *p, long len)
 {
   char *s = buf + *index;
@@ -40,3 +44,106 @@ int ei_encode_binary(char *buf, int *index, const void *p, long len)
   return 0; 
 }
 
+int ei_encode_bitstring(char *buf, int *index,
+                        const char *p,
+                        size_t bitoffs,
+                        size_t bits)
+{
+  char *s = buf + *index;
+  char *s0 = s;
+  size_t bytes = (bits + 7) / 8;
+  char last_bits = bits % 8;
+
+  if (!buf) s += last_bits ? 6 : 5;
+  else {
+      char* tagp = s++;
+      put32be(s, bytes);
+      if (last_bits) {
+          *tagp = ERL_BIT_BINARY_EXT;
+          put8(s, last_bits);
+      }
+      else
+          *tagp = ERL_BINARY_EXT;
+
+      copy_bits((const unsigned char*)p, bitoffs, (unsigned char*)s, bits);
+  }
+  s += bytes;
+
+  *index += s-s0;
+
+  return 0;
+}
+
+
+/*
+ * MAKE_MASK(n) constructs a mask with n bits.
+ * Example: MAKE_MASK(3) returns the binary number 00000111.
+ */
+#define MAKE_MASK(n) ((((unsigned) 1) << (n))-1)
+
+
+static
+void copy_bits(const unsigned char* src, /* Base pointer to source. */
+	       size_t soffs,	         /* Bit offset for source relative to src. */
+	       unsigned char* dst,	 /* Destination. */
+	       size_t n)	         /* Number of bits to copy. */
+{
+    unsigned rmask;
+    unsigned count;
+    unsigned deoffs;
+    unsigned bits;
+    unsigned bits1;
+    unsigned rshift;
+
+    if (n == 0)
+        return;
+
+    deoffs = n & 7;
+    rmask = deoffs ? (MAKE_MASK(deoffs) << (8-deoffs)) : 0;
+
+    if (soffs == 0) {
+        unsigned nbytes = (n + 7) / 8;
+        memcpy(dst, src, nbytes);
+        if (rmask)
+            dst[nbytes-1] &= rmask;
+        return;
+    }
+
+    src += soffs / 8;
+    soffs &= 7;
+
+    if (n < 8) {     /* Less than one byte */
+        bits = (*src << soffs);
+        if (soffs+n > 8) {
+            src++;
+            bits |= (*src >> (8 - soffs));
+        }
+        *dst = bits & rmask;
+	return;
+    }
+
+    count = n >> 3;
+
+    rshift = 8 - soffs;
+    bits = *src;
+    if (soffs + n > 8) {
+        src++;
+    }
+
+    while (count--) {
+        bits1 = bits << soffs;
+        bits = *src;
+        src++;
+        *dst = bits1 | (bits >> rshift);
+        dst++;
+    }
+
+    if (rmask) {
+        bits1 = bits << soffs;
+        if ((rmask << rshift) & 0xff) {
+            bits = *src;
+            bits1 |= (bits >> rshift);
+        }
+        *dst = bits1 & rmask;
+    }
+}