1#include <string.h>
2#include <jim.h>
3
4/* Provides the [pack] and [unpack] commands to pack and unpack
5 * a binary string to/from arbitrary width integers and strings.
6 *
7 * This may be used to implement the [binary] command.
8 */
9
10/**
11 * Big endian bit test.
12 *
13 * Considers 'bitvect' as a big endian bit stream and returns
14 * bit 'b' as zero or non-zero.
15 */
16static int JimTestBitBigEndian(const unsigned char *bitvec, int b)
17{
18    div_t pos = div(b, 8);
19    return bitvec[pos.quot] & (1 << (7 - pos.rem));
20}
21
22/**
23 * Little endian bit test.
24 *
25 * Considers 'bitvect' as a little endian bit stream and returns
26 * bit 'b' as zero or non-zero.
27 */
28static int JimTestBitLittleEndian(const unsigned char *bitvec, int b)
29{
30    div_t pos = div(b, 8);
31    return bitvec[pos.quot] & (1 << pos.rem);
32}
33
34/**
35 * Sign extends the given value, 'n' of width 'width' bits.
36 *
37 * For example, sign extending 0x80 with a width of 8, produces -128
38 */
39static jim_wide JimSignExtend(jim_wide n, int width)
40{
41    if (width == sizeof(jim_wide) * 8) {
42        /* Can't sign extend the maximum size integer */
43        return n;
44    }
45    if (n & ((jim_wide)1 << (width - 1))) {
46        /* Need to extend */
47        n -= ((jim_wide)1 << width);
48    }
49
50    return n;
51}
52
53/**
54 * Big endian integer extraction.
55 *
56 * Considers 'bitvect' as a big endian bit stream.
57 * Returns an integer of the given width (in bits)
58 * starting at the given position (in bits).
59 *
60 * The pos/width must represent bits inside bitvec,
61 * and the width be no more than the width of jim_wide.
62 */
63static jim_wide JimBitIntBigEndian(const unsigned char *bitvec, int pos, int width)
64{
65    jim_wide result = 0;
66    int i;
67
68    /* Aligned, byte extraction */
69    if (pos % 8 == 0 && width % 8 == 0) {
70        for (i = 0; i < width; i += 8) {
71            result = (result << 8) + bitvec[(pos + i) / 8];
72        }
73        return result;
74    }
75
76    /* Unaligned */
77    for (i = 0; i < width; i++) {
78        if (JimTestBitBigEndian(bitvec, pos + width - i - 1)) {
79            result |= ((jim_wide)1 << i);
80        }
81    }
82
83    return result;
84}
85
86/**
87 * Little endian integer extraction.
88 *
89 * Like JimBitIntBigEndian() but considers 'bitvect' as a little endian bit stream.
90 */
91static jim_wide JimBitIntLittleEndian(const unsigned char *bitvec, int pos, int width)
92{
93    jim_wide result = 0;
94    int i;
95
96    /* Aligned, byte extraction */
97    if (pos % 8 == 0 && width % 8 == 0) {
98        for (i = 0; i < width; i += 8) {
99            result += (jim_wide)bitvec[(pos + i) / 8] << i;
100        }
101        return result;
102    }
103
104    /* Unaligned */
105    for (i = 0; i < width; i++) {
106        if (JimTestBitLittleEndian(bitvec, pos + i)) {
107            result |= ((jim_wide)1 << i);
108        }
109    }
110
111    return result;
112}
113
114/**
115 * Big endian bit set.
116 *
117 * Considers 'bitvect' as a big endian bit stream and sets
118 * bit 'b' to 'bit'
119 */
120static void JimSetBitBigEndian(unsigned char *bitvec, int b, int bit)
121{
122    div_t pos = div(b, 8);
123    if (bit) {
124        bitvec[pos.quot] |= (1 << (7 - pos.rem));
125    }
126    else {
127        bitvec[pos.quot] &= ~(1 << (7 - pos.rem));
128    }
129}
130
131/**
132 * Little endian bit set.
133 *
134 * Considers 'bitvect' as a little endian bit stream and sets
135 * bit 'b' to 'bit'
136 */
137static void JimSetBitLittleEndian(unsigned char *bitvec, int b, int bit)
138{
139    div_t pos = div(b, 8);
140    if (bit) {
141        bitvec[pos.quot] |= (1 << pos.rem);
142    }
143    else {
144        bitvec[pos.quot] &= ~(1 << pos.rem);
145    }
146}
147
148/**
149 * Big endian integer packing.
150 *
151 * Considers 'bitvect' as a big endian bit stream.
152 * Packs integer 'value' of the given width (in bits)
153 * starting at the given position (in bits).
154 *
155 * The pos/width must represent bits inside bitvec,
156 * and the width be no more than the width of jim_wide.
157 */
158static void JimSetBitsIntBigEndian(unsigned char *bitvec, jim_wide value, int pos, int width)
159{
160    int i;
161
162    /* Common fast option */
163    if (pos % 8 == 0 && width == 8) {
164        bitvec[pos / 8] = value;
165        return;
166    }
167
168    for (i = 0; i < width; i++) {
169        int bit = !!(value & ((jim_wide)1 << i));
170        JimSetBitBigEndian(bitvec, pos + width - i - 1, bit);
171    }
172}
173
174/**
175 * Little endian version of JimSetBitsIntBigEndian()
176 */
177static void JimSetBitsIntLittleEndian(unsigned char *bitvec, jim_wide value, int pos, int width)
178{
179    int i;
180
181    /* Common fast option */
182    if (pos % 8 == 0 && width == 8) {
183        bitvec[pos / 8] = value;
184        return;
185    }
186
187    for (i = 0; i < width; i++) {
188        int bit = !!(value & ((jim_wide)1 << i));
189        JimSetBitLittleEndian(bitvec, pos + i, bit);
190    }
191}
192
193/**
194 * [unpack]
195 *
196 * Usage: unpack binvalue -intbe|-intle|-uintbe|-uintle|-str bitpos bitwidth
197 *
198 * Unpacks bits from $binvalue at bit position $bitpos and with $bitwidth.
199 * Interprets the value according to the type and returns it.
200 */
201static int Jim_UnpackCmd(Jim_Interp *interp, int argc, Jim_Obj *const *argv)
202{
203    int option;
204    static const char * const options[] = { "-intbe", "-intle", "-uintbe", "-uintle", "-str", NULL };
205    enum { OPT_INTBE, OPT_INTLE, OPT_UINTBE, OPT_UINTLE, OPT_STR, };
206    jim_wide pos;
207    jim_wide width;
208
209    if (argc != 5) {
210        Jim_WrongNumArgs(interp, 1, argv, "binvalue -intbe|-intle|-uintbe|-uintle|-str bitpos bitwidth");
211        return JIM_ERR;
212    }
213    if (Jim_GetEnum(interp, argv[2], options, &option, NULL, JIM_ERRMSG) != JIM_OK) {
214        return JIM_ERR;
215    }
216
217    if (Jim_GetWide(interp, argv[3], &pos) != JIM_OK) {
218        return JIM_ERR;
219    }
220    if (Jim_GetWide(interp, argv[4], &width) != JIM_OK) {
221        return JIM_ERR;
222    }
223
224    if (option == OPT_STR) {
225        int len;
226        const char *str = Jim_GetString(argv[1], &len);
227
228        if (width % 8 || pos % 8) {
229            Jim_SetResultString(interp, "string field is not on a byte boundary", -1);
230            return JIM_ERR;
231        }
232
233        if (pos >= 0 && width > 0 && pos < len * 8) {
234            if (pos + width > len * 8) {
235                width = len * 8 - pos;
236            }
237            Jim_SetResultString(interp, str + pos / 8, width / 8);
238        }
239        return JIM_OK;
240    }
241    else {
242        int len;
243        const unsigned char *str = (const unsigned char *)Jim_GetString(argv[1], &len);
244        jim_wide result = 0;
245
246        if (width > sizeof(jim_wide) * 8) {
247            Jim_SetResultFormatted(interp, "int field is too wide: %#s", argv[4]);
248            return JIM_ERR;
249        }
250
251        if (pos >= 0 && width > 0 && pos < len * 8) {
252            if (pos + width > len * 8) {
253                width = len * 8 - pos;
254            }
255            if (option == OPT_INTBE || option == OPT_UINTBE) {
256                result = JimBitIntBigEndian(str, pos, width);
257            }
258            else {
259                result = JimBitIntLittleEndian(str, pos, width);
260            }
261            if (option == OPT_INTBE || option == OPT_INTLE) {
262                result = JimSignExtend(result, width);
263            }
264        }
265        Jim_SetResultInt(interp, result);
266        return JIM_OK;
267    }
268}
269
270/**
271 * [pack]
272 *
273 * Usage: pack varname value -intle|-intbe|-str width ?bitoffset?
274 *
275 * Packs the binary representation of 'value' into the variable of the given name.
276 * The value is packed according to the given type, width and bitoffset.
277 * The variable is created if necessary (like [append])
278 * Ihe variable is expanded if necessary
279 */
280static int Jim_PackCmd(Jim_Interp *interp, int argc, Jim_Obj *const *argv)
281{
282    int option;
283    static const char * const options[] = { "-intle", "-intbe", "-str", NULL };
284    enum { OPT_LE, OPT_BE, OPT_STR };
285    jim_wide pos = 0;
286    jim_wide width;
287    jim_wide value;
288    Jim_Obj *stringObjPtr;
289    int len;
290    int freeobj = 0;
291
292    if (argc != 5 && argc != 6) {
293        Jim_WrongNumArgs(interp, 1, argv, "varName value -intle|-intbe|-str bitwidth ?bitoffset?");
294        return JIM_ERR;
295    }
296    if (Jim_GetEnum(interp, argv[3], options, &option, NULL, JIM_ERRMSG) != JIM_OK) {
297        return JIM_ERR;
298    }
299    if (option != OPT_STR && Jim_GetWide(interp, argv[2], &value) != JIM_OK) {
300        return JIM_ERR;
301    }
302    if (Jim_GetWide(interp, argv[4], &width) != JIM_OK) {
303        return JIM_ERR;
304    }
305    if (width <= 0 || (option == OPT_STR && width % 8) || (option != OPT_STR && width > sizeof(jim_wide) * 8)) {
306        Jim_SetResultFormatted(interp, "bad bitwidth: %#s", argv[5]);
307        return JIM_ERR;
308    }
309    if (argc == 6) {
310        if (Jim_GetWide(interp, argv[5], &pos) != JIM_OK) {
311            return JIM_ERR;
312        }
313        if (pos < 0 || (option == OPT_STR && pos % 8)) {
314            Jim_SetResultFormatted(interp, "bad bitoffset: %#s", argv[5]);
315            return JIM_ERR;
316        }
317    }
318
319    stringObjPtr = Jim_GetVariable(interp, argv[1], JIM_UNSHARED);
320    if (!stringObjPtr) {
321        /* Create the string if it doesn't exist */
322        stringObjPtr = Jim_NewEmptyStringObj(interp);
323        freeobj = 1;
324    }
325    else if (Jim_IsShared(stringObjPtr)) {
326        freeobj = 1;
327        stringObjPtr = Jim_DuplicateObj(interp, stringObjPtr);
328    }
329
330    len = Jim_Length(stringObjPtr) * 8;
331
332    /* Extend the string as necessary first */
333    while (len < pos + width) {
334        Jim_AppendString(interp, stringObjPtr, "", 1);
335        len += 8;
336    }
337
338    Jim_SetResultInt(interp, pos + width);
339
340    /* Now set the bits. Note that the the string *must* have no non-string rep
341     * since we are writing the bytes directly.
342     */
343    Jim_AppendString(interp, stringObjPtr, "", 0);
344
345    if (option == OPT_BE) {
346        JimSetBitsIntBigEndian((unsigned char *)stringObjPtr->bytes, value, pos, width);
347    }
348    else if (option == OPT_LE) {
349        JimSetBitsIntLittleEndian((unsigned char *)stringObjPtr->bytes, value, pos, width);
350    }
351    else {
352        pos /= 8;
353        width /= 8;
354
355        if (width > Jim_Length(argv[2])) {
356            width = Jim_Length(argv[2]);
357        }
358        memcpy(stringObjPtr->bytes + pos, Jim_GetString(argv[2], NULL), width);
359        /* No padding is needed since the string is already extended */
360    }
361
362    if (Jim_SetVariable(interp, argv[1], stringObjPtr) != JIM_OK) {
363        if (freeobj) {
364            Jim_FreeNewObj(interp, stringObjPtr);
365            return JIM_ERR;
366        }
367    }
368    return JIM_OK;
369}
370
371int Jim_packInit(Jim_Interp *interp)
372{
373    if (Jim_PackageProvide(interp, "pack", "1.0", JIM_ERRMSG)) {
374        return JIM_ERR;
375    }
376
377    Jim_CreateCommand(interp, "unpack", Jim_UnpackCmd, NULL, NULL);
378    Jim_CreateCommand(interp, "pack", Jim_PackCmd, NULL, NULL);
379    return JIM_OK;
380}
381