381 lines
10 KiB
C
Executable File
381 lines
10 KiB
C
Executable File
#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;
|
|
}
|