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