release/src/router/usbmodeswitch/jim/jim-pack.c

   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  */
  16 static 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  */
  28 static 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  */
  39 static 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  */
  63 static 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  */
  91 static 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  */
 120 static 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  */
 137 static 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  */
 158 static 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  */
 177 static 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  */
 201 static 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  */
 280 static 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
 371 int 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 }