bitstring.ml

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