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