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