bitmatch.ml

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