diff options
Diffstat (limited to 'jim-pack.c')
| -rw-r--r-- | jim-pack.c | 380 |
1 files changed, 380 insertions, 0 deletions
diff --git a/jim-pack.c b/jim-pack.c new file mode 100644 index 0000000..f517a92 --- /dev/null +++ b/jim-pack.c @@ -0,0 +1,380 @@ +#include <string.h> +#include <jim.h> + +/* Provides the [pack] and [unpack] commands to pack and unpack + * a binary string to/from arbitrary width integers and strings. + * + * This may be used to implement the [binary] command. + */ + +/** + * Big endian bit test. + * + * Considers 'bitvect' as a big endian bit stream and returns + * bit 'b' as zero or non-zero. + */ +static int JimTestBitBigEndian(const unsigned char *bitvec, int b) +{ + div_t pos = div(b, 8); + return bitvec[pos.quot] & (1 << (7 - pos.rem)); +} + +/** + * Little endian bit test. + * + * Considers 'bitvect' as a little endian bit stream and returns + * bit 'b' as zero or non-zero. + */ +static int JimTestBitLittleEndian(const unsigned char *bitvec, int b) +{ + div_t pos = div(b, 8); + return bitvec[pos.quot] & (1 << pos.rem); +} + +/** + * Sign extends the given value, 'n' of width 'width' bits. + * + * For example, sign extending 0x80 with a width of 8, produces -128 + */ +static jim_wide JimSignExtend(jim_wide n, int width) +{ + if (width == sizeof(jim_wide) * 8) { + /* Can't sign extend the maximum size integer */ + return n; + } + if (n & ((jim_wide)1 << (width - 1))) { + /* Need to extend */ + n -= ((jim_wide)1 << width); + } + + return n; +} + +/** + * Big endian integer extraction. + * + * Considers 'bitvect' as a big endian bit stream. + * Returns an integer of the given width (in bits) + * starting at the given position (in bits). + * + * The pos/width must represent bits inside bitvec, + * and the width be no more than the width of jim_wide. + */ +static jim_wide JimBitIntBigEndian(const unsigned char *bitvec, int pos, int width) +{ + jim_wide result = 0; + int i; + + /* Aligned, byte extraction */ + if (pos % 8 == 0 && width % 8 == 0) { + for (i = 0; i < width; i += 8) { + result = (result << 8) + bitvec[(pos + i) / 8]; + } + return result; + } + + /* Unaligned */ + for (i = 0; i < width; i++) { + if (JimTestBitBigEndian(bitvec, pos + width - i - 1)) { + result |= ((jim_wide)1 << i); + } + } + + return result; +} + +/** + * Little endian integer extraction. + * + * Like JimBitIntBigEndian() but considers 'bitvect' as a little endian bit stream. + */ +static jim_wide JimBitIntLittleEndian(const unsigned char *bitvec, int pos, int width) +{ + jim_wide result = 0; + int i; + + /* Aligned, byte extraction */ + if (pos % 8 == 0 && width % 8 == 0) { + for (i = 0; i < width; i += 8) { + result += (jim_wide)bitvec[(pos + i) / 8] << i; + } + return result; + } + + /* Unaligned */ + for (i = 0; i < width; i++) { + if (JimTestBitLittleEndian(bitvec, pos + i)) { + result |= ((jim_wide)1 << i); + } + } + + return result; +} + +/** + * Big endian bit set. + * + * Considers 'bitvect' as a big endian bit stream and sets + * bit 'b' to 'bit' + */ +static void JimSetBitBigEndian(unsigned char *bitvec, int b, int bit) +{ + div_t pos = div(b, 8); + if (bit) { + bitvec[pos.quot] |= (1 << (7 - pos.rem)); + } + else { + bitvec[pos.quot] &= ~(1 << (7 - pos.rem)); + } +} + +/** + * Little endian bit set. + * + * Considers 'bitvect' as a little endian bit stream and sets + * bit 'b' to 'bit' + */ +static void JimSetBitLittleEndian(unsigned char *bitvec, int b, int bit) +{ + div_t pos = div(b, 8); + if (bit) { + bitvec[pos.quot] |= (1 << pos.rem); + } + else { + bitvec[pos.quot] &= ~(1 << pos.rem); + } +} + +/** + * Big endian integer packing. + * + * Considers 'bitvect' as a big endian bit stream. + * Packs integer 'value' of the given width (in bits) + * starting at the given position (in bits). + * + * The pos/width must represent bits inside bitvec, + * and the width be no more than the width of jim_wide. + */ +static void JimSetBitsIntBigEndian(unsigned char *bitvec, jim_wide value, int pos, int width) +{ + int i; + + /* Common fast option */ + if (pos % 8 == 0 && width == 8) { + bitvec[pos / 8] = value; + return; + } + + for (i = 0; i < width; i++) { + int bit = !!(value & ((jim_wide)1 << i)); + JimSetBitBigEndian(bitvec, pos + width - i - 1, bit); + } +} + +/** + * Little endian version of JimSetBitsIntBigEndian() + */ +static void JimSetBitsIntLittleEndian(unsigned char *bitvec, jim_wide value, int pos, int width) +{ + int i; + + /* Common fast option */ + if (pos % 8 == 0 && width == 8) { + bitvec[pos / 8] = value; + return; + } + + for (i = 0; i < width; i++) { + int bit = !!(value & ((jim_wide)1 << i)); + JimSetBitLittleEndian(bitvec, pos + i, bit); + } +} + +/** + * [unpack] + * + * Usage: unpack binvalue -intbe|-intle|-uintbe|-uintle|-str bitpos bitwidth + * + * Unpacks bits from $binvalue at bit position $bitpos and with $bitwidth. + * Interprets the value according to the type and returns it. + */ +static int Jim_UnpackCmd(Jim_Interp *interp, int argc, Jim_Obj *const *argv) +{ + int option; + static const char const *options[] = { "-intbe", "-intle", "-uintbe", "-uintle", "-str", NULL }; + enum { OPT_INTBE, OPT_INTLE, OPT_UINTBE, OPT_UINTLE, OPT_STR, }; + jim_wide pos; + jim_wide width; + + if (argc != 5) { + Jim_WrongNumArgs(interp, 1, argv, "binvalue -intbe|-intle|-uintbe|-uintle|-str bitpos bitwidth"); + return JIM_ERR; + } + if (Jim_GetEnum(interp, argv[2], options, &option, NULL, JIM_ERRMSG) != JIM_OK) { + return JIM_ERR; + } + + if (Jim_GetWide(interp, argv[3], &pos) != JIM_OK) { + return JIM_ERR; + } + if (Jim_GetWide(interp, argv[4], &width) != JIM_OK) { + return JIM_ERR; + } + + if (option == OPT_STR) { + int len; + const char *str = Jim_GetString(argv[1], &len); + + if (width % 8 || pos % 8) { + Jim_SetResultString(interp, "string field is not on a byte boundary", -1); + return JIM_ERR; + } + + if (pos >= 0 && width > 0 && pos < len * 8) { + if (pos + width > len * 8) { + width = len * 8 - pos; + } + Jim_SetResultString(interp, str + pos / 8, width / 8); + } + return JIM_OK; + } + else { + int len; + const unsigned char *str = (const unsigned char *)Jim_GetString(argv[1], &len); + jim_wide result = 0; + + if (width > sizeof(jim_wide) * 8) { + Jim_SetResultFormatted(interp, "int field is too wide: %#s", argv[4]); + return JIM_ERR; + } + + if (pos >= 0 && width > 0 && pos < len * 8) { + if (pos + width > len * 8) { + width = len * 8 - pos; + } + if (option == OPT_INTBE || option == OPT_UINTBE) { + result = JimBitIntBigEndian(str, pos, width); + } + else { + result = JimBitIntLittleEndian(str, pos, width); + } + if (option == OPT_INTBE || option == OPT_INTLE) { + result = JimSignExtend(result, width); + } + } + Jim_SetResultInt(interp, result); + return JIM_OK; + } +} + +/** + * [pack] + * + * Usage: pack varname value -intle|-intbe|-str width ?bitoffset? + * + * Packs the binary representation of 'value' into the variable of the given name. + * The value is packed according to the given type, width and bitoffset. + * The variable is created if necessary (like [append]) + * Ihe variable is expanded if necessary + */ +static int Jim_PackCmd(Jim_Interp *interp, int argc, Jim_Obj *const *argv) +{ + int option; + static const char const *options[] = { "-intle", "-intbe", "-str", NULL }; + enum { OPT_LE, OPT_BE, OPT_STR }; + jim_wide pos = 0; + jim_wide width; + jim_wide value; + Jim_Obj *stringObjPtr; + int len; + int freeobj = 0; + + if (argc != 5 && argc != 6) { + Jim_WrongNumArgs(interp, 1, argv, "varName value -intle|-intbe|-str bitwidth ?bitoffset?"); + return JIM_ERR; + } + if (Jim_GetEnum(interp, argv[3], options, &option, NULL, JIM_ERRMSG) != JIM_OK) { + return JIM_ERR; + } + if (option != OPT_STR && Jim_GetWide(interp, argv[2], &value) != JIM_OK) { + return JIM_ERR; + } + if (Jim_GetWide(interp, argv[4], &width) != JIM_OK) { + return JIM_ERR; + } + if (width <= 0 || (option == OPT_STR && width % 8) || (option != OPT_STR && width > sizeof(jim_wide) * 8)) { + Jim_SetResultFormatted(interp, "bad bitwidth: %#s", argv[5]); + return JIM_ERR; + } + if (argc == 6) { + if (Jim_GetWide(interp, argv[5], &pos) != JIM_OK) { + return JIM_ERR; + } + if (pos < 0 || (option == OPT_STR && pos % 8)) { + Jim_SetResultFormatted(interp, "bad bitoffset: %#s", argv[5]); + return JIM_ERR; + } + } + + stringObjPtr = Jim_GetVariable(interp, argv[1], JIM_UNSHARED); + if (!stringObjPtr) { + /* Create the string if it doesn't exist */ + stringObjPtr = Jim_NewEmptyStringObj(interp); + freeobj = 1; + } + else if (Jim_IsShared(stringObjPtr)) { + freeobj = 1; + stringObjPtr = Jim_DuplicateObj(interp, stringObjPtr); + } + + len = Jim_Length(stringObjPtr) * 8; + + /* Extend the string as necessary first */ + while (len < pos + width) { + Jim_AppendString(interp, stringObjPtr, "", 1); + len += 8; + } + + Jim_SetResultInt(interp, pos + width); + + /* Now set the bits. Note that the the string *must* have no non-string rep + * since we are writing the bytes directly. + */ + Jim_AppendString(interp, stringObjPtr, "", 0); + + if (option == OPT_BE) { + JimSetBitsIntBigEndian((unsigned char *)stringObjPtr->bytes, value, pos, width); + } + else if (option == OPT_LE) { + JimSetBitsIntLittleEndian((unsigned char *)stringObjPtr->bytes, value, pos, width); + } + else { + pos /= 8; + width /= 8; + + if (width > Jim_Length(argv[2])) { + width = Jim_Length(argv[2]); + } + memcpy(stringObjPtr->bytes + pos, Jim_GetString(argv[2], NULL), width); + /* No padding is needed since the string is already extended */ + } + + if (Jim_SetVariable(interp, argv[1], stringObjPtr) != JIM_OK) { + if (freeobj) { + Jim_FreeNewObj(interp, stringObjPtr); + return JIM_ERR; + } + } + return JIM_OK; +} + +int Jim_packInit(Jim_Interp *interp) +{ + if (Jim_PackageProvide(interp, "pack", "1.0", JIM_ERRMSG)) { + return JIM_ERR; + } + + Jim_CreateCommand(interp, "unpack", Jim_UnpackCmd, NULL, NULL); + Jim_CreateCommand(interp, "pack", Jim_PackCmd, NULL, NULL); + return JIM_OK; +} |