bitmatch.ml

   1 (* Bitmatch library.
   2  * Copyright (C) 2008 Red Hat Inc., Richard W.M. Jones
   3  *
   4  * This library is free software; you can redistribute it and/or
   5  * modify it under the terms of the GNU Lesser General Public
   6  * License as published by the Free Software Foundation; either
   7  * version 2 of the License, or (at your option) any later version,
   8  * with the OCaml linking exception described in COPYING.LIB.
   9  *
  10  * This library is distributed in the hope that it will be useful,
  11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  13  * Lesser General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU Lesser General Public
  16  * License along with this library; if not, write to the Free Software
  17  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
  18  *
  19  * $Id$
  20  *)
  21
  22 open Printf
  23
  24 include Bitmatch_types
  25 include Bitmatch_config
  26
  27 (* Enable runtime debug messages.  Must also have been enabled
  28  * in pa_bitmatch.ml.
  29  *)
  30 let debug = ref false
  31
  32 (* Exceptions. *)
  33 exception Construct_failure of string * string * int * int
  34
  35 (* A bitstring is simply the data itself (as a string), and the
  36  * bitoffset and the bitlength within the string.  Note offset/length
  37  * are counted in bits, not bytes.
  38  *)
  39 type bitstring = string * int * int
  40
  41 (* Functions to create and load bitstrings. *)
  42 let empty_bitstring = "", 0, 0
  43
  44 let make_bitstring len c =
  45   if len >= 0 then String.make ((len+7) lsr 3) c, 0, len
  46   else
  47     invalid_arg (
  48       sprintf "make_bitstring/create_bitstring: len %d < 0" len
  49     )
  50
  51 let create_bitstring len = make_bitstring len '\000'
  52
  53 let zeroes_bitstring = create_bitstring
  54
  55 let ones_bitstring len = make_bitstring len '\xff'
  56
  57 let bitstring_of_string str = str, 0, String.length str lsl 3
  58
  59 let bitstring_of_chan chan =
  60   let tmpsize = 16384 in
  61   let buf = Buffer.create tmpsize in
  62   let tmp = String.create tmpsize in
  63   let n = ref 0 in
  64   while n := input chan tmp 0 tmpsize; !n > 0 do
  65     Buffer.add_substring buf tmp 0 !n;
  66   done;
  67   Buffer.contents buf, 0, Buffer.length buf lsl 3
  68
  69 let bitstring_of_chan_max chan max =
  70   let tmpsize = 16384 in
  71   let buf = Buffer.create tmpsize in
  72   let tmp = String.create tmpsize in
  73   let len = ref 0 in
  74   let rec loop () =
  75     if !len < max then (
  76       let r = min tmpsize (max - !len) in
  77       let n = input chan tmp 0 r in
  78       if n > 0 then (
  79         Buffer.add_substring buf tmp 0 n;
  80         len := !len + n;
  81         loop ()
  82       )
  83     )
  84   in
  85   loop ();
  86   Buffer.contents buf, 0, !len lsl 3
  87
  88 let bitstring_of_file_descr fd =
  89   let tmpsize = 16384 in
  90   let buf = Buffer.create tmpsize in
  91   let tmp = String.create tmpsize in
  92   let n = ref 0 in
  93   while n := Unix.read fd tmp 0 tmpsize; !n > 0 do
  94     Buffer.add_substring buf tmp 0 !n;
  95   done;
  96   Buffer.contents buf, 0, Buffer.length buf lsl 3
  97
  98 let bitstring_of_file_descr_max fd max =
  99   let tmpsize = 16384 in
 100   let buf = Buffer.create tmpsize in
 101   let tmp = String.create tmpsize in
 102   let len = ref 0 in
 103   let rec loop () =
 104     if !len < max then (
 105       let r = min tmpsize (max - !len) in
 106       let n = Unix.read fd tmp 0 r in
 107       if n > 0 then (
 108         Buffer.add_substring buf tmp 0 n;
 109         len := !len + n;
 110         loop ()
 111       )
 112     )
 113   in
 114   loop ();
 115   Buffer.contents buf, 0, !len lsl 3
 116
 117 let bitstring_of_file fname =
 118   let chan = open_in_bin fname in
 119   try
 120     let bs = bitstring_of_chan chan in
 121     close_in chan;
 122     bs
 123   with exn ->
 124     close_in chan;
 125     raise exn
 126
 127 let bitstring_length (_, _, len) = len
 128
 129 (*----------------------------------------------------------------------*)
 130 (* Bitwise functions.
 131  *
 132  * We try to isolate all bitwise functions within these modules.
 133  *)
 134
 135 module I = struct
 136   (* Bitwise operations on ints.  Note that we assume int <= 31 bits. *)
 137   let (<<) = (lsl)
 138   let (>>) = (lsr)
 139   external to_int : int -> int = "%identity"
 140   let zero = 0
 141   let one = 1
 142   let minus_one = -1
 143   let ff = 0xff
 144
 145   (* Create a mask so many bits wide. *)
 146   let mask bits =
 147     if bits < 30 then
 148       pred (one << bits)
 149     else if bits = 30 then
 150       max_int
 151     else if bits = 31 then
 152       minus_one
 153     else
 154       invalid_arg "Bitmatch.I.mask"
 155
 156   (* Byte swap an int of a given size. *)
 157   let byteswap v bits =
 158     if bits <= 8 then v
 159     else if bits <= 16 then (
 160       let shift = bits-8 in
 161       let v1 = v >> shift in
 162       let v2 = (v land (mask shift)) << 8 in
 163       v2 lor v1
 164     ) else if bits <= 24 then (
 165       let shift = bits - 16 in
 166       let v1 = v >> (8+shift) in
 167       let v2 = ((v >> shift) land ff) << 8 in
 168       let v3 = (v land (mask shift)) << 16 in
 169       v3 lor v2 lor v1
 170     ) else (
 171       let shift = bits - 24 in
 172       let v1 = v >> (16+shift) in
 173       let v2 = ((v >> (8+shift)) land ff) << 8 in
 174       let v3 = ((v >> shift) land ff) << 16 in
 175       let v4 = (v land (mask shift)) << 24 in
 176       v4 lor v3 lor v2 lor v1
 177     )
 178
 179   (* Check a value is in range 0 .. 2^bits-1. *)
 180   let range_unsigned v bits =
 181     let mask = lnot (mask bits) in
 182     (v land mask) = zero
 183
 184   (* Call function g on the top bits, then f on each full byte
 185    * (big endian - so start at top).
 186    *)
 187   let rec map_bytes_be g f v bits =
 188     if bits >= 8 then (
 189       map_bytes_be g f (v >> 8) (bits-8);
 190       let lsb = v land ff in
 191       f (to_int lsb)
 192     ) else if bits > 0 then (
 193       let lsb = v land (mask bits) in
 194       g (to_int lsb) bits
 195     )
 196 end
 197
 198 module I32 = struct
 199   (* Bitwise operations on int32s.  Note we try to keep it as similar
 200    * as possible to the I module above, to make it easier to track
 201    * down bugs.
 202    *)
 203   let (<<) = Int32.shift_left
 204   let (>>) = Int32.shift_right_logical
 205   let (land) = Int32.logand
 206   let (lor) = Int32.logor
 207   let lnot = Int32.lognot
 208   let pred = Int32.pred
 209   let max_int = Int32.max_int
 210   let to_int = Int32.to_int
 211   let zero = Int32.zero
 212   let one = Int32.one
 213   let minus_one = Int32.minus_one
 214   let ff = 0xff_l
 215
 216   (* Create a mask so many bits wide. *)
 217   let mask bits =
 218     if bits < 31 then
 219       pred (one << bits)
 220     else if bits = 31 then
 221       max_int
 222     else if bits = 32 then
 223       minus_one
 224     else
 225       invalid_arg "Bitmatch.I32.mask"
 226
 227   (* Byte swap an int of a given size. *)
 228   let byteswap v bits =
 229     if bits <= 8 then v
 230     else if bits <= 16 then (
 231       let shift = bits-8 in
 232       let v1 = v >> shift in
 233       let v2 = (v land (mask shift)) << 8 in
 234       v2 lor v1
 235     ) else if bits <= 24 then (
 236       let shift = bits - 16 in
 237       let v1 = v >> (8+shift) in
 238       let v2 = ((v >> shift) land ff) << 8 in
 239       let v3 = (v land (mask shift)) << 16 in
 240       v3 lor v2 lor v1
 241     ) else (
 242       let shift = bits - 24 in
 243       let v1 = v >> (16+shift) in
 244       let v2 = ((v >> (8+shift)) land ff) << 8 in
 245       let v3 = ((v >> shift) land ff) << 16 in
 246       let v4 = (v land (mask shift)) << 24 in
 247       v4 lor v3 lor v2 lor v1
 248     )
 249
 250   (* Check a value is in range 0 .. 2^bits-1. *)
 251   let range_unsigned v bits =
 252     let mask = lnot (mask bits) in
 253     (v land mask) = zero
 254
 255   (* Call function g on the top bits, then f on each full byte
 256    * (big endian - so start at top).
 257    *)
 258   let rec map_bytes_be g f v bits =
 259     if bits >= 8 then (
 260       map_bytes_be g f (v >> 8) (bits-8);
 261       let lsb = v land ff in
 262       f (to_int lsb)
 263     ) else if bits > 0 then (
 264       let lsb = v land (mask bits) in
 265       g (to_int lsb) bits
 266     )
 267 end
 268
 269 module I64 = struct
 270   (* Bitwise operations on int64s.  Note we try to keep it as similar
 271    * as possible to the I/I32 modules above, to make it easier to track
 272    * down bugs.
 273    *)
 274   let (<<) = Int64.shift_left
 275   let (>>) = Int64.shift_right_logical
 276   let (land) = Int64.logand
 277   let (lor) = Int64.logor
 278   let lnot = Int64.lognot
 279   let pred = Int64.pred
 280   let max_int = Int64.max_int
 281   let to_int = Int64.to_int
 282   let zero = Int64.zero
 283   let one = Int64.one
 284   let minus_one = Int64.minus_one
 285   let ff = 0xff_L
 286
 287   (* Create a mask so many bits wide. *)
 288   let mask bits =
 289     if bits < 63 then
 290       pred (one << bits)
 291     else if bits = 63 then
 292       max_int
 293     else if bits = 64 then
 294       minus_one
 295     else
 296       invalid_arg "Bitmatch.I64.mask"
 297
 298   (* Byte swap an int of a given size. *)
 299   (* let byteswap v bits = *)
 300
 301   (* Check a value is in range 0 .. 2^bits-1. *)
 302   let range_unsigned v bits =
 303     let mask = lnot (mask bits) in
 304     (v land mask) = zero
 305
 306   (* Call function g on the top bits, then f on each full byte
 307    * (big endian - so start at top).
 308    *)
 309   let rec map_bytes_be g f v bits =
 310     if bits >= 8 then (
 311       map_bytes_be g f (v >> 8) (bits-8);
 312       let lsb = v land ff in
 313       f (to_int lsb)
 314     ) else if bits > 0 then (
 315       let lsb = v land (mask bits) in
 316       g (to_int lsb) bits
 317     )
 318 end
 319
 320 (*----------------------------------------------------------------------*)
 321 (* Extraction functions.
 322  *
 323  * NB: internal functions, called from the generated macros, and
 324  * the parameters should have been checked for sanity already).
 325  *)
 326
 327 (* Bitstrings. *)
 328 let extract_bitstring data off len flen =
 329   (data, off, flen), off+flen, len-flen
 330
 331 let extract_remainder data off len =
 332   (data, off, len), off+len, 0
 333
 334 (* Extract and convert to numeric.  A single bit is returned as
 335  * a boolean.  There are no endianness or signedness considerations.
 336  *)
 337 let extract_bit data off len _ =        (* final param is always 1 *)
 338   let byteoff = off lsr 3 in
 339   let bitmask = 1 lsl (7 - (off land 7)) in
 340   let b = Char.code data.[byteoff] land bitmask <> 0 in
 341   b, off+1, len-1
 342
 343 (* Returns 8 bit unsigned aligned bytes from the string.
 344  * If the string ends then this returns 0's.
 345  *)
 346 let _get_byte data byteoff strlen =
 347   if strlen > byteoff then Char.code data.[byteoff] else 0
 348 let _get_byte32 data byteoff strlen =
 349   if strlen > byteoff then Int32.of_int (Char.code data.[byteoff]) else 0l
 350 let _get_byte64 data byteoff strlen =
 351   if strlen > byteoff then Int64.of_int (Char.code data.[byteoff]) else 0L
 352
 353 (* Extract [2..8] bits.  Because the result fits into a single
 354  * byte we don't have to worry about endianness, only signedness.
 355  *)
 356 let extract_char_unsigned data off len flen =
 357   let byteoff = off lsr 3 in
 358
 359   (* Optimize the common (byte-aligned) case. *)
 360   if off land 7 = 0 then (
 361     let byte = Char.code data.[byteoff] in
 362     byte lsr (8 - flen), off+flen, len-flen
 363   ) else (
 364     (* Extract the 16 bits at byteoff and byteoff+1 (note that the
 365      * second byte might not exist in the original string).
 366      *)
 367     let strlen = String.length data in
 368
 369     let word =
 370       (_get_byte data byteoff strlen lsl 8) +
 371         _get_byte data (byteoff+1) strlen in
 372
 373     (* Mask off the top bits. *)
 374     let bitmask = (1 lsl (16 - (off land 7))) - 1 in
 375     let word = word land bitmask in
 376     (* Shift right to get rid of the bottom bits. *)
 377     let shift = 16 - ((off land 7) + flen) in
 378     let word = word lsr shift in
 379
 380     word, off+flen, len-flen
 381   )
 382
 383 (* Extract [9..31] bits.  We have to consider endianness and signedness. *)
 384 let extract_int_be_unsigned data off len flen =
 385   let byteoff = off lsr 3 in
 386
 387   let strlen = String.length data in
 388
 389   let word =
 390     (* Optimize the common (byte-aligned) case. *)
 391     if off land 7 = 0 then (
 392       let word =
 393         (_get_byte data byteoff strlen lsl 23) +
 394           (_get_byte data (byteoff+1) strlen lsl 15) +
 395           (_get_byte data (byteoff+2) strlen lsl 7) +
 396           (_get_byte data (byteoff+3) strlen lsr 1) in
 397       word lsr (31 - flen)
 398     ) else if flen <= 24 then (
 399       (* Extract the 31 bits at byteoff .. byteoff+3. *)
 400       let word =
 401         (_get_byte data byteoff strlen lsl 23) +
 402           (_get_byte data (byteoff+1) strlen lsl 15) +
 403           (_get_byte data (byteoff+2) strlen lsl 7) +
 404           (_get_byte data (byteoff+3) strlen lsr 1) in
 405       (* Mask off the top bits. *)
 406       let bitmask = (1 lsl (31 - (off land 7))) - 1 in
 407       let word = word land bitmask in
 408       (* Shift right to get rid of the bottom bits. *)
 409       let shift = 31 - ((off land 7) + flen) in
 410       word lsr shift
 411     ) else (
 412       (* Extract the next 31 bits, slow method. *)
 413       let word =
 414         let c0, off, len = extract_char_unsigned data off len 8 in
 415         let c1, off, len = extract_char_unsigned data off len 8 in
 416         let c2, off, len = extract_char_unsigned data off len 8 in
 417         let c3, off, len = extract_char_unsigned data off len 7 in
 418         (c0 lsl 23) + (c1 lsl 15) + (c2 lsl 7) + c3 in
 419       word lsr (31 - flen)
 420     ) in
 421   word, off+flen, len-flen
 422
 423 let extract_int_le_unsigned data off len flen =
 424   let v, off, len = extract_int_be_unsigned data off len flen in
 425   let v = I.byteswap v flen in
 426   v, off, len
 427
 428 let extract_int_ne_unsigned =
 429   if nativeendian = BigEndian
 430   then extract_int_be_unsigned
 431   else extract_int_le_unsigned
 432
 433 let extract_int_ee_unsigned = function
 434   | BigEndian -> extract_int_be_unsigned
 435   | LittleEndian -> extract_int_le_unsigned
 436   | NativeEndian -> extract_int_ne_unsigned
 437
 438 let _make_int32_be c0 c1 c2 c3 =
 439   Int32.logor
 440     (Int32.logor
 441        (Int32.logor
 442           (Int32.shift_left c0 24)
 443           (Int32.shift_left c1 16))
 444        (Int32.shift_left c2 8))
 445     c3
 446
 447 let _make_int32_le c0 c1 c2 c3 =
 448   Int32.logor
 449     (Int32.logor
 450        (Int32.logor
 451           (Int32.shift_left c3 24)
 452           (Int32.shift_left c2 16))
 453        (Int32.shift_left c1 8))
 454     c0
 455
 456 (* Extract exactly 32 bits.  We have to consider endianness and signedness. *)
 457 let extract_int32_be_unsigned data off len flen =
 458   let byteoff = off lsr 3 in
 459
 460   let strlen = String.length data in
 461
 462   let word =
 463     (* Optimize the common (byte-aligned) case. *)
 464     if off land 7 = 0 then (
 465       let word =
 466         let c0 = _get_byte32 data byteoff strlen in
 467         let c1 = _get_byte32 data (byteoff+1) strlen in
 468         let c2 = _get_byte32 data (byteoff+2) strlen in
 469         let c3 = _get_byte32 data (byteoff+3) strlen in
 470         _make_int32_be c0 c1 c2 c3 in
 471       Int32.shift_right_logical word (32 - flen)
 472     ) else (
 473       (* Extract the next 32 bits, slow method. *)
 474       let word =
 475         let c0, off, len = extract_char_unsigned data off len 8 in
 476         let c1, off, len = extract_char_unsigned data off len 8 in
 477         let c2, off, len = extract_char_unsigned data off len 8 in
 478         let c3, _, _ = extract_char_unsigned data off len 8 in
 479         let c0 = Int32.of_int c0 in
 480         let c1 = Int32.of_int c1 in
 481         let c2 = Int32.of_int c2 in
 482         let c3 = Int32.of_int c3 in
 483         _make_int32_be c0 c1 c2 c3 in
 484       Int32.shift_right_logical word (32 - flen)
 485     ) in
 486   word, off+flen, len-flen
 487
 488 let extract_int32_le_unsigned data off len flen =
 489   let v, off, len = extract_int32_be_unsigned data off len flen in
 490   let v = I32.byteswap v flen in
 491   v, off, len
 492
 493 let extract_int32_ne_unsigned =
 494   if nativeendian = BigEndian
 495   then extract_int32_be_unsigned
 496   else extract_int32_le_unsigned
 497
 498 let extract_int32_ee_unsigned = function
 499   | BigEndian -> extract_int32_be_unsigned
 500   | LittleEndian -> extract_int32_le_unsigned
 501   | NativeEndian -> extract_int32_ne_unsigned
 502
 503 let _make_int64_be c0 c1 c2 c3 c4 c5 c6 c7 =
 504   Int64.logor
 505     (Int64.logor
 506        (Int64.logor
 507           (Int64.logor
 508              (Int64.logor
 509                 (Int64.logor
 510                    (Int64.logor
 511                       (Int64.shift_left c0 56)
 512                       (Int64.shift_left c1 48))
 513                    (Int64.shift_left c2 40))
 514                 (Int64.shift_left c3 32))
 515              (Int64.shift_left c4 24))
 516           (Int64.shift_left c5 16))
 517        (Int64.shift_left c6 8))
 518     c7
 519
 520 let _make_int64_le c0 c1 c2 c3 c4 c5 c6 c7 =
 521   _make_int64_be c7 c6 c5 c4 c3 c2 c1 c0
 522
 523 (* Extract [1..64] bits.  We have to consider endianness and signedness. *)
 524 let extract_int64_be_unsigned data off len flen =
 525   let byteoff = off lsr 3 in
 526
 527   let strlen = String.length data in
 528
 529   let word =
 530     (* Optimize the common (byte-aligned) case. *)
 531     if off land 7 = 0 then (
 532       let word =
 533         let c0 = _get_byte64 data byteoff strlen in
 534         let c1 = _get_byte64 data (byteoff+1) strlen in
 535         let c2 = _get_byte64 data (byteoff+2) strlen in
 536         let c3 = _get_byte64 data (byteoff+3) strlen in
 537         let c4 = _get_byte64 data (byteoff+4) strlen in
 538         let c5 = _get_byte64 data (byteoff+5) strlen in
 539         let c6 = _get_byte64 data (byteoff+6) strlen in
 540         let c7 = _get_byte64 data (byteoff+7) strlen in
 541         _make_int64_be c0 c1 c2 c3 c4 c5 c6 c7 in
 542       Int64.shift_right_logical word (64 - flen)
 543     ) else (
 544       (* Extract the next 64 bits, slow method. *)
 545       let word =
 546         let c0, off, len = extract_char_unsigned data off len 8 in
 547         let c1, off, len = extract_char_unsigned data off len 8 in
 548         let c2, off, len = extract_char_unsigned data off len 8 in
 549         let c3, off, len = extract_char_unsigned data off len 8 in
 550         let c4, off, len = extract_char_unsigned data off len 8 in
 551         let c5, off, len = extract_char_unsigned data off len 8 in
 552         let c6, off, len = extract_char_unsigned data off len 8 in
 553         let c7, _, _ = extract_char_unsigned data off len 8 in
 554         let c0 = Int64.of_int c0 in
 555         let c1 = Int64.of_int c1 in
 556         let c2 = Int64.of_int c2 in
 557         let c3 = Int64.of_int c3 in
 558         let c4 = Int64.of_int c4 in
 559         let c5 = Int64.of_int c5 in
 560         let c6 = Int64.of_int c6 in
 561         let c7 = Int64.of_int c7 in
 562         _make_int64_be c0 c1 c2 c3 c4 c5 c6 c7 in
 563       Int64.shift_right_logical word (64 - flen)
 564     ) in
 565   word, off+flen, len-flen
 566
 567 let extract_int64_le_unsigned data off len flen =
 568   let byteoff = off lsr 3 in
 569
 570   let strlen = String.length data in
 571
 572   let word =
 573     (* Optimize the common (byte-aligned) case. *)
 574     if off land 7 = 0 then (
 575       let word =
 576         let c0 = _get_byte64 data byteoff strlen in
 577         let c1 = _get_byte64 data (byteoff+1) strlen in
 578         let c2 = _get_byte64 data (byteoff+2) strlen in
 579         let c3 = _get_byte64 data (byteoff+3) strlen in
 580         let c4 = _get_byte64 data (byteoff+4) strlen in
 581         let c5 = _get_byte64 data (byteoff+5) strlen in
 582         let c6 = _get_byte64 data (byteoff+6) strlen in
 583         let c7 = _get_byte64 data (byteoff+7) strlen in
 584         _make_int64_le c0 c1 c2 c3 c4 c5 c6 c7 in
 585       Int64.logand word (I64.mask flen)
 586     ) else (
 587       (* Extract the next 64 bits, slow method. *)
 588       let word =
 589         let c0, off, len = extract_char_unsigned data off len 8 in
 590         let c1, off, len = extract_char_unsigned data off len 8 in
 591         let c2, off, len = extract_char_unsigned data off len 8 in
 592         let c3, off, len = extract_char_unsigned data off len 8 in
 593         let c4, off, len = extract_char_unsigned data off len 8 in
 594         let c5, off, len = extract_char_unsigned data off len 8 in
 595         let c6, off, len = extract_char_unsigned data off len 8 in
 596         let c7, _, _ = extract_char_unsigned data off len 8 in
 597         let c0 = Int64.of_int c0 in
 598         let c1 = Int64.of_int c1 in
 599         let c2 = Int64.of_int c2 in
 600         let c3 = Int64.of_int c3 in
 601         let c4 = Int64.of_int c4 in
 602         let c5 = Int64.of_int c5 in
 603         let c6 = Int64.of_int c6 in
 604         let c7 = Int64.of_int c7 in
 605         _make_int64_le c0 c1 c2 c3 c4 c5 c6 c7 in
 606       Int64.logand word (I64.mask flen)
 607     ) in
 608   word, off+flen, len-flen
 609
 610 let extract_int64_ne_unsigned =
 611   if nativeendian = BigEndian
 612   then extract_int64_be_unsigned
 613   else extract_int64_le_unsigned
 614
 615 let extract_int64_ee_unsigned = function
 616   | BigEndian -> extract_int64_be_unsigned
 617   | LittleEndian -> extract_int64_le_unsigned
 618   | NativeEndian -> extract_int64_ne_unsigned
 619
 620 (*----------------------------------------------------------------------*)
 621 (* Constructor functions. *)
 622
 623 module Buffer = struct
 624   type t = {
 625     buf : Buffer.t;
 626     mutable len : int;                  (* Length in bits. *)
 627     (* Last byte in the buffer (if len is not aligned).  We store
 628      * it outside the buffer because buffers aren't mutable.
 629      *)
 630     mutable last : int;
 631   }
 632
 633   let create () =
 634     (* XXX We have almost enough information in the generator to
 635      * choose a good initial size.
 636      *)
 637     { buf = Buffer.create 128; len = 0; last = 0 }
 638
 639   let contents { buf = buf; len = len; last = last } =
 640     let data =
 641       if len land 7 = 0 then
 642         Buffer.contents buf
 643       else
 644         Buffer.contents buf ^ (String.make 1 (Char.chr last)) in
 645     data, 0, len
 646
 647   (* Add exactly 8 bits. *)
 648   let add_byte ({ buf = buf; len = len; last = last } as t) byte =
 649     if byte < 0 || byte > 255 then invalid_arg "Bitmatch.Buffer.add_byte";
 650     let shift = len land 7 in
 651     if shift = 0 then
 652       (* Target buffer is byte-aligned. *)
 653       Buffer.add_char buf (Char.chr byte)
 654     else (
 655       (* Target buffer is unaligned.  'last' is meaningful. *)
 656       let first = byte lsr shift in
 657       let second = (byte lsl (8 - shift)) land 0xff in
 658       Buffer.add_char buf (Char.chr (last lor first));
 659       t.last <- second
 660     );
 661     t.len <- t.len + 8
 662
 663   (* Add exactly 1 bit. *)
 664   let add_bit ({ buf = buf; len = len; last = last } as t) bit =
 665     let shift = 7 - (len land 7) in
 666     if shift > 0 then
 667       (* Somewhere in the middle of 'last'. *)
 668       t.last <- last lor ((if bit then 1 else 0) lsl shift)
 669     else (
 670       (* Just a single spare bit in 'last'. *)
 671       let last = last lor if bit then 1 else 0 in
 672       Buffer.add_char buf (Char.chr last);
 673       t.last <- 0
 674     );
 675     t.len <- len + 1
 676
 677   (* Add a small number of bits (definitely < 8).  This uses a loop
 678    * to call add_bit so it's slow.
 679    *)
 680   let _add_bits t c slen =
 681     if slen < 1 || slen >= 8 then invalid_arg "Bitmatch.Buffer._add_bits";
 682     for i = slen-1 downto 0 do
 683       let bit = c land (1 lsl i) <> 0 in
 684       add_bit t bit
 685     done
 686
 687   let add_bits ({ buf = buf; len = len } as t) str slen =
 688     if slen > 0 then (
 689       if len land 7 = 0 then (
 690         if slen land 7 = 0 then
 691           (* Common case - everything is byte-aligned. *)
 692           Buffer.add_substring buf str 0 (slen lsr 3)
 693         else (
 694           (* Target buffer is aligned.  Copy whole bytes then leave the
 695            * remaining bits in last.
 696            *)
 697           let slenbytes = slen lsr 3 in
 698           if slenbytes > 0 then Buffer.add_substring buf str 0 slenbytes;
 699           let last = Char.code str.[slenbytes] in (* last char *)
 700           let mask = 0xff lsl (8 - (slen land 7)) in
 701           t.last <- last land mask
 702         );
 703         t.len <- len + slen
 704       ) else (
 705         (* Target buffer is unaligned.  Copy whole bytes using
 706          * add_byte which knows how to deal with an unaligned
 707          * target buffer, then call add_bit for the remaining < 8 bits.
 708          *
 709          * XXX This is going to be dog-slow.
 710          *)
 711         let slenbytes = slen lsr 3 in
 712         for i = 0 to slenbytes-1 do
 713           let byte = Char.code str.[i] in
 714           add_byte t byte
 715         done;
 716         let bitsleft = slen - (slenbytes lsl 3) in
 717         if bitsleft > 0 then (
 718           let c = Char.code str.[slenbytes] in
 719           for i = 0 to bitsleft - 1 do
 720             let bit = c land (0x80 lsr i) <> 0 in
 721             add_bit t bit
 722           done
 723         )
 724       );
 725     )
 726 end
 727
 728 (* Construct a single bit. *)
 729 let construct_bit buf b _ _ =
 730   Buffer.add_bit buf b
 731
 732 (* Construct a field, flen = [2..8]. *)
 733 let construct_char_unsigned buf v flen exn =
 734   let max_val = 1 lsl flen in
 735   if v < 0 || v >= max_val then raise exn;
 736   if flen = 8 then
 737     Buffer.add_byte buf v
 738   else
 739     Buffer._add_bits buf v flen
 740
 741 (* Construct a field of up to 31 bits. *)
 742 let construct_int_be_unsigned buf v flen exn =
 743   (* Check value is within range. *)
 744   if not (I.range_unsigned v flen) then raise exn;
 745   (* Add the bytes. *)
 746   I.map_bytes_be (Buffer._add_bits buf) (Buffer.add_byte buf) v flen
 747
 748 let construct_int_ne_unsigned =
 749   if nativeendian = BigEndian
 750   then construct_int_be_unsigned
 751   else (*construct_int_le_unsigned*)
 752     fun _ _ _ _ -> failwith "construct_int_le_unsigned"
 753
 754 let construct_int_ee_unsigned = function
 755   | BigEndian -> construct_int_be_unsigned
 756   | LittleEndian -> (*construct_int_le_unsigned*)
 757       (fun _ _ _ _ -> failwith "construct_int_le_unsigned")
 758   | NativeEndian -> construct_int_ne_unsigned
 759
 760 (* Construct a field of exactly 32 bits. *)
 761 let construct_int32_be_unsigned buf v flen _ =
 762   Buffer.add_byte buf
 763     (Int32.to_int (Int32.shift_right_logical v 24));
 764   Buffer.add_byte buf
 765     (Int32.to_int ((Int32.logand (Int32.shift_right_logical v 16) 0xff_l)));
 766   Buffer.add_byte buf
 767     (Int32.to_int ((Int32.logand (Int32.shift_right_logical v 8) 0xff_l)));
 768   Buffer.add_byte buf
 769     (Int32.to_int (Int32.logand v 0xff_l))
 770
 771 let construct_int32_le_unsigned buf v flen _ =
 772   Buffer.add_byte buf
 773     (Int32.to_int (Int32.logand v 0xff_l));
 774   Buffer.add_byte buf
 775     (Int32.to_int ((Int32.logand (Int32.shift_right_logical v 8) 0xff_l)));
 776   Buffer.add_byte buf
 777     (Int32.to_int ((Int32.logand (Int32.shift_right_logical v 16) 0xff_l)));
 778   Buffer.add_byte buf
 779     (Int32.to_int (Int32.shift_right_logical v 24))
 780
 781 let construct_int32_ne_unsigned =
 782   if nativeendian = BigEndian
 783   then construct_int32_be_unsigned
 784   else construct_int32_le_unsigned
 785
 786 let construct_int32_ee_unsigned = function
 787   | BigEndian -> construct_int32_be_unsigned
 788   | LittleEndian -> construct_int32_le_unsigned
 789   | NativeEndian -> construct_int32_ne_unsigned
 790
 791 (* Construct a field of up to 64 bits. *)
 792 let construct_int64_be_unsigned buf v flen exn =
 793   (* Check value is within range. *)
 794   if not (I64.range_unsigned v flen) then raise exn;
 795   (* Add the bytes. *)
 796   I64.map_bytes_be (Buffer._add_bits buf) (Buffer.add_byte buf) v flen
 797
 798 let construct_int64_ne_unsigned =
 799   if nativeendian = BigEndian
 800   then construct_int64_be_unsigned
 801   else (*construct_int64_le_unsigned*)
 802     fun _ _ _ _ -> failwith "construct_int64_le_unsigned"
 803
 804 let construct_int64_ee_unsigned = function
 805   | BigEndian -> construct_int64_be_unsigned
 806   | LittleEndian -> (*construct_int64_le_unsigned*)
 807       (fun _ _ _ _ -> failwith "construct_int64_le_unsigned")
 808   | NativeEndian -> construct_int64_ne_unsigned
 809
 810 (* Construct from a string of bytes, exact multiple of 8 bits
 811  * in length of course.
 812  *)
 813 let construct_string buf str =
 814   let len = String.length str in
 815   Buffer.add_bits buf str (len lsl 3)
 816
 817 (* Construct from a bitstring. *)
 818 let construct_bitstring buf (data, off, len) =
 819   (* Add individual bits until we get to the next byte boundary of
 820    * the underlying string.
 821    *)
 822   let blen = 7 - ((off + 7) land 7) in
 823   let blen = min blen len in
 824   let rec loop off len blen =
 825     if blen = 0 then (off, len)
 826     else (
 827       let b, off, len = extract_bit data off len 1 in
 828       Buffer.add_bit buf b;
 829       loop off len (blen-1)
 830     )
 831   in
 832   let off, len = loop off len blen in
 833   assert (len = 0 || (off land 7) = 0);
 834
 835   (* Add the remaining 'len' bits. *)
 836   let data =
 837     let off = off lsr 3 in
 838     (* XXX dangerous allocation *)
 839     if off = 0 then data
 840     else String.sub data off (String.length data - off) in
 841
 842   Buffer.add_bits buf data len
 843
 844 (*----------------------------------------------------------------------*)
 845 (* Extract a string from a bitstring. *)
 846
 847 let string_of_bitstring (data, off, len) =
 848   if off land 7 = 0 && len land 7 = 0 then
 849     (* Easy case: everything is byte-aligned. *)
 850     String.sub data (off lsr 3) (len lsr 3)
 851   else (
 852     (* Bit-twiddling case. *)
 853     let strlen = (len + 7) lsr 3 in
 854     let str = String.make strlen '\000' in
 855     let rec loop data off len i =
 856       if len >= 8 then (
 857         let c, off, len = extract_char_unsigned data off len 8 in
 858         str.[i] <- Char.chr c;
 859         loop data off len (i+1)
 860       ) else if len > 0 then (
 861         let c, _, _ = extract_char_unsigned data off len len in
 862         str.[i] <- Char.chr (c lsl (8-len))
 863       )
 864     in
 865     loop data off len 0;
 866     str
 867   )
 868
 869 (* To channel. *)
 870
 871 let bitstring_to_chan ((data, off, len) as bits) chan =
 872   (* Fail if the bitstring length isn't a multiple of 8. *)
 873   if len land 7 <> 0 then invalid_arg "bitstring_to_chan";
 874
 875   if off land 7 = 0 then
 876     (* Easy case: string is byte-aligned. *)
 877     output chan data (off lsr 3) (len lsr 3)
 878   else (
 879     (* Bit-twiddling case: reuse string_of_bitstring *)
 880     let str = string_of_bitstring bits in
 881     output_string chan str
 882   )
 883
 884 let bitstring_to_file bits filename =
 885   let chan = open_out_bin filename in
 886   try
 887     bitstring_to_chan bits chan;
 888     close_out chan
 889   with exn ->
 890     close_out chan;
 891     raise exn
 892
 893 (*----------------------------------------------------------------------*)
 894 (* Display functions. *)
 895
 896 let isprint c =
 897   let c = Char.code c in
 898   c >= 32 && c < 127
 899
 900 let hexdump_bitstring chan (data, off, len) =
 901   let count = ref 0 in
 902   let off = ref off in
 903   let len = ref len in
 904   let linelen = ref 0 in
 905   let linechars = String.make 16 ' ' in
 906
 907   fprintf chan "00000000  ";
 908
 909   while !len > 0 do
 910     let bits = min !len 8 in
 911     let byte, off', len' = extract_char_unsigned data !off !len bits in
 912     off := off'; len := len';
 913
 914     let byte = byte lsl (8-bits) in
 915     fprintf chan "%02x " byte;
 916
 917     incr count;
 918     linechars.[!linelen] <-
 919       (let c = Char.chr byte in
 920        if isprint c then c else '.');
 921     incr linelen;
 922     if !linelen = 8 then fprintf chan " ";
 923     if !linelen = 16 then (
 924       fprintf chan " |%s|\n%08x  " linechars !count;
 925       linelen := 0;
 926       for i = 0 to 15 do linechars.[i] <- ' ' done
 927     )
 928   done;
 929
 930   if !linelen > 0 then (
 931     let skip = (16 - !linelen) * 3 + if !linelen < 8 then 1 else 0 in
 932     for i = 0 to skip-1 do fprintf chan " " done;
 933     fprintf chan " |%s|\n%!" linechars
 934   ) else
 935     fprintf chan "\n%!"