diff options
Diffstat (limited to 'lib/erl_interface/src/encode/encode_binary.c')
-rw-r--r-- | lib/erl_interface/src/encode/encode_binary.c | 107 |
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; + } +} |