1 /*
2 * %CopyrightBegin%
3 *
4 * Copyright Ericsson AB 1998-2020. All Rights Reserved.
5 *
6 * Licensed under the Apache License, Version 2.0 (the "License");
7 * you may not use this file except in compliance with the License.
8 * You may obtain a copy of the License at
9 *
10 * http://www.apache.org/licenses/LICENSE-2.0
11 *
12 * Unless required by applicable law or agreed to in writing, software
13 * distributed under the License is distributed on an "AS IS" BASIS,
14 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15 * See the License for the specific language governing permissions and
16 * limitations under the License.
17 *
18 * %CopyrightEnd%
19 */
20 #include <string.h>
21 #include "eidef.h"
22 #include "eiext.h"
23 #include "putget.h"
24
25 static void copy_bits(const unsigned char* src, size_t soffs,
26 unsigned char* dst, size_t n);
27
28
ei_encode_binary(char * buf,int * index,const void * p,long len)29 int ei_encode_binary(char *buf, int *index, const void *p, long len)
30 {
31 char *s = buf + *index;
32 char *s0 = s;
33
34 if (!buf) s += 5;
35 else {
36 put8(s,ERL_BINARY_EXT);
37 put32be(s,len);
38 memmove(s,p,len);
39 }
40 s += len;
41
42 *index += s-s0;
43
44 return 0;
45 }
46
ei_encode_bitstring(char * buf,int * index,const char * p,size_t bitoffs,size_t bits)47 int ei_encode_bitstring(char *buf, int *index,
48 const char *p,
49 size_t bitoffs,
50 size_t bits)
51 {
52 char *s = buf + *index;
53 char *s0 = s;
54 size_t bytes = (bits + 7) / 8;
55 char last_bits = bits % 8;
56
57 if (!buf) s += last_bits ? 6 : 5;
58 else {
59 char* tagp = s++;
60 put32be(s, bytes);
61 if (last_bits) {
62 *tagp = ERL_BIT_BINARY_EXT;
63 put8(s, last_bits);
64 }
65 else
66 *tagp = ERL_BINARY_EXT;
67
68 copy_bits((const unsigned char*)p, bitoffs, (unsigned char*)s, bits);
69 }
70 s += bytes;
71
72 *index += s-s0;
73
74 return 0;
75 }
76
77
78 /*
79 * MAKE_MASK(n) constructs a mask with n bits.
80 * Example: MAKE_MASK(3) returns the binary number 00000111.
81 */
82 #define MAKE_MASK(n) ((((unsigned) 1) << (n))-1)
83
84
85 static
copy_bits(const unsigned char * src,size_t soffs,unsigned char * dst,size_t n)86 void copy_bits(const unsigned char* src, /* Base pointer to source. */
87 size_t soffs, /* Bit offset for source relative to src. */
88 unsigned char* dst, /* Destination. */
89 size_t n) /* Number of bits to copy. */
90 {
91 unsigned rmask;
92 unsigned count;
93 unsigned deoffs;
94 unsigned bits;
95 unsigned bits1;
96 unsigned rshift;
97
98 if (n == 0)
99 return;
100
101 deoffs = n & 7;
102 rmask = deoffs ? (MAKE_MASK(deoffs) << (8-deoffs)) : 0;
103
104 if (soffs == 0) {
105 unsigned nbytes = (n + 7) / 8;
106 memcpy(dst, src, nbytes);
107 if (rmask)
108 dst[nbytes-1] &= rmask;
109 return;
110 }
111
112 src += soffs / 8;
113 soffs &= 7;
114
115 if (n < 8) { /* Less than one byte */
116 bits = (*src << soffs);
117 if (soffs+n > 8) {
118 src++;
119 bits |= (*src >> (8 - soffs));
120 }
121 *dst = bits & rmask;
122 return;
123 }
124
125 count = n >> 3;
126
127 rshift = 8 - soffs;
128 bits = *src;
129 if (soffs + n > 8) {
130 src++;
131 }
132
133 while (count--) {
134 bits1 = bits << soffs;
135 bits = *src;
136 src++;
137 *dst = bits1 | (bits >> rshift);
138 dst++;
139 }
140
141 if (rmask) {
142 bits1 = bits << soffs;
143 if ((rmask << rshift) & 0xff) {
144 bits = *src;
145 bits1 |= (bits >> rshift);
146 }
147 *dst = bits1 & rmask;
148 }
149 }
150