bitmatch.ml

   1 (* Bitmatch library.
   2  * $Id: bitmatch.ml,v 1.5 2008-04-01 17:05:37 rjones Exp $
   3  *)
   4
   5 open Printf
   6
   7 (* Enable runtime debug messages.  Must also have been enabled
   8  * in pa_bitmatch.ml.
   9  *)
  10 let debug = ref false
  11
  12 (* Exceptions. *)
  13 exception Construct_failure of string * string * int * int
  14
  15 (* A bitstring is simply the data itself (as a string), and the
  16  * bitoffset and the bitlength within the string.  Note offset/length
  17  * are counted in bits, not bytes.
  18  *)
  19 type bitstring = string * int * int
  20
  21 (* Functions to create and load bitstrings. *)
  22 let empty_bitstring = "", 0, 0
  23
  24 let make_bitstring len c = String.make ((len+7) lsr 3) c, 0, len
  25
  26 let create_bitstring len = make_bitstring len '\000'
  27
  28 let bitstring_of_chan chan =
  29   let tmpsize = 16384 in
  30   let buf = Buffer.create tmpsize in
  31   let tmp = String.create tmpsize in
  32   let n = ref 0 in
  33   while n := input chan tmp 0 tmpsize; !n > 0 do
  34     Buffer.add_substring buf tmp 0 !n;
  35   done;
  36   Buffer.contents buf, 0, Buffer.length buf lsl 3
  37
  38 let bitstring_of_file fname =
  39   let chan = open_in_bin fname in
  40   let bs = bitstring_of_chan chan in
  41   close_in chan;
  42   bs
  43
  44 let bitstring_length (_, _, len) = len
  45
  46 (*----------------------------------------------------------------------*)
  47 (* Extraction functions.
  48  *
  49  * NB: internal functions, called from the generated macros, and
  50  * the parameters should have been checked for sanity already).
  51  *)
  52
  53 (* Bitstrings. *)
  54 let extract_bitstring data off len flen =
  55   (data, off, flen), off+flen, len-flen
  56
  57 let extract_remainder data off len =
  58   (data, off, len), off+len, 0
  59
  60 (* Extract and convert to numeric.  A single bit is returned as
  61  * a boolean.  There are no endianness or signedness considerations.
  62  *)
  63 let extract_bit data off len _ =        (* final param is always 1 *)
  64   let byteoff = off lsr 3 in
  65   let bitmask = 1 lsl (7 - (off land 7)) in
  66   let b = Char.code data.[byteoff] land bitmask <> 0 in
  67   b, off+1, len-1
  68
  69 (* Returns 8 bit unsigned aligned bytes from the string.
  70  * If the string ends then this returns 0's.
  71  *)
  72 let _get_byte data byteoff strlen =
  73   if strlen > byteoff then Char.code data.[byteoff] else 0
  74 let _get_byte32 data byteoff strlen =
  75   if strlen > byteoff then Int32.of_int (Char.code data.[byteoff]) else 0l
  76 let _get_byte64 data byteoff strlen =
  77   if strlen > byteoff then Int64.of_int (Char.code data.[byteoff]) else 0L
  78
  79 (* Extract [2..8] bits.  Because the result fits into a single
  80  * byte we don't have to worry about endianness, only signedness.
  81  *)
  82 let extract_char_unsigned data off len flen =
  83   let byteoff = off lsr 3 in
  84
  85   (* Optimize the common (byte-aligned) case. *)
  86   if off land 7 = 0 then (
  87     let byte = Char.code data.[byteoff] in
  88     byte lsr (8 - flen), off+flen, len-flen
  89   ) else (
  90     (* Extract the 16 bits at byteoff and byteoff+1 (note that the
  91      * second byte might not exist in the original string).
  92      *)
  93     let strlen = String.length data in
  94
  95     let word =
  96       (_get_byte data byteoff strlen lsl 8) +
  97         _get_byte data (byteoff+1) strlen in
  98
  99     (* Mask off the top bits. *)
 100     let bitmask = (1 lsl (16 - (off land 7))) - 1 in
 101     let word = word land bitmask in
 102     (* Shift right to get rid of the bottom bits. *)
 103     let shift = 16 - ((off land 7) + flen) in
 104     let word = word lsr shift in
 105
 106     word, off+flen, len-flen
 107   )
 108
 109 (* Extract [9..31] bits.  We have to consider endianness and signedness. *)
 110 let extract_int_be_unsigned data off len flen =
 111   let byteoff = off lsr 3 in
 112
 113   let strlen = String.length data in
 114
 115   let word =
 116     (* Optimize the common (byte-aligned) case. *)
 117     if off land 7 = 0 then (
 118       let word =
 119         (_get_byte data byteoff strlen lsl 23) +
 120           (_get_byte data (byteoff+1) strlen lsl 15) +
 121           (_get_byte data (byteoff+2) strlen lsl 7) +
 122           (_get_byte data (byteoff+3) strlen lsr 1) in
 123       word lsr (31 - flen)
 124     ) else if flen <= 24 then (
 125       (* Extract the 31 bits at byteoff .. byteoff+3. *)
 126       let word =
 127         (_get_byte data byteoff strlen lsl 23) +
 128           (_get_byte data (byteoff+1) strlen lsl 15) +
 129           (_get_byte data (byteoff+2) strlen lsl 7) +
 130           (_get_byte data (byteoff+3) strlen lsr 1) in
 131       (* Mask off the top bits. *)
 132       let bitmask = (1 lsl (31 - (off land 7))) - 1 in
 133       let word = word land bitmask in
 134       (* Shift right to get rid of the bottom bits. *)
 135       let shift = 31 - ((off land 7) + flen) in
 136       word lsr shift
 137     ) else (
 138       (* Extract the next 31 bits, slow method. *)
 139       let word =
 140         let c0, off, len = extract_char_unsigned data off len 8 in
 141         let c1, off, len = extract_char_unsigned data off len 8 in
 142         let c2, off, len = extract_char_unsigned data off len 8 in
 143         let c3, off, len = extract_char_unsigned data off len 7 in
 144         (c0 lsl 23) + (c1 lsl 15) + (c2 lsl 7) + c3 in
 145       word lsr (31 - flen)
 146     ) in
 147   word, off+flen, len-flen
 148
 149 let _make_int32_be c0 c1 c2 c3 =
 150   Int32.logor
 151     (Int32.logor
 152        (Int32.logor
 153           (Int32.shift_left c0 24)
 154           (Int32.shift_left c1 16))
 155        (Int32.shift_left c2 8))
 156     c3
 157
 158 (* Extract exactly 32 bits.  We have to consider endianness and signedness. *)
 159 let extract_int32_be_unsigned data off len flen =
 160   let byteoff = off lsr 3 in
 161
 162   let strlen = String.length data in
 163
 164   let word =
 165     (* Optimize the common (byte-aligned) case. *)
 166     if off land 7 = 0 then (
 167       let word =
 168         let c0 = _get_byte32 data byteoff strlen in
 169         let c1 = _get_byte32 data (byteoff+1) strlen in
 170         let c2 = _get_byte32 data (byteoff+2) strlen in
 171         let c3 = _get_byte32 data (byteoff+3) strlen in
 172         _make_int32_be c0 c1 c2 c3 in
 173       Int32.shift_right_logical word (32 - flen)
 174     ) else (
 175       (* Extract the next 32 bits, slow method. *)
 176       let word =
 177         let c0, off, len = extract_char_unsigned data off len 8 in
 178         let c1, off, len = extract_char_unsigned data off len 8 in
 179         let c2, off, len = extract_char_unsigned data off len 8 in
 180         let c3, _, _ = extract_char_unsigned data off len 8 in
 181         let c0 = Int32.of_int c0 in
 182         let c1 = Int32.of_int c1 in
 183         let c2 = Int32.of_int c2 in
 184         let c3 = Int32.of_int c3 in
 185         _make_int32_be c0 c1 c2 c3 in
 186       Int32.shift_right_logical word (32 - flen)
 187     ) in
 188   word, off+flen, len-flen
 189
 190 let _make_int64_be c0 c1 c2 c3 c4 c5 c6 c7 =
 191   Int64.logor
 192     (Int64.logor
 193        (Int64.logor
 194           (Int64.logor
 195              (Int64.logor
 196                 (Int64.logor
 197                    (Int64.logor
 198                       (Int64.shift_left c0 56)
 199                       (Int64.shift_left c1 48))
 200                    (Int64.shift_left c2 40))
 201                 (Int64.shift_left c3 32))
 202              (Int64.shift_left c4 24))
 203           (Int64.shift_left c5 16))
 204        (Int64.shift_left c6 8))
 205     c7
 206
 207 (* Extract [1..64] bits.  We have to consider endianness and signedness. *)
 208 let extract_int64_be_unsigned data off len flen =
 209   let byteoff = off lsr 3 in
 210
 211   let strlen = String.length data in
 212
 213   let word =
 214     (* Optimize the common (byte-aligned) case. *)
 215     if off land 7 = 0 then (
 216       let word =
 217         let c0 = _get_byte64 data byteoff strlen in
 218         let c1 = _get_byte64 data (byteoff+1) strlen in
 219         let c2 = _get_byte64 data (byteoff+2) strlen in
 220         let c3 = _get_byte64 data (byteoff+3) strlen in
 221         let c4 = _get_byte64 data (byteoff+4) strlen in
 222         let c5 = _get_byte64 data (byteoff+5) strlen in
 223         let c6 = _get_byte64 data (byteoff+6) strlen in
 224         let c7 = _get_byte64 data (byteoff+7) strlen in
 225         _make_int64_be c0 c1 c2 c3 c4 c5 c6 c7 in
 226       Int64.shift_right_logical word (64 - flen)
 227     ) else (
 228       (* Extract the next 64 bits, slow method. *)
 229       let word =
 230         let c0, off, len = extract_char_unsigned data off len 8 in
 231         let c1, off, len = extract_char_unsigned data off len 8 in
 232         let c2, off, len = extract_char_unsigned data off len 8 in
 233         let c3, off, len = extract_char_unsigned data off len 8 in
 234         let c4, off, len = extract_char_unsigned data off len 8 in
 235         let c5, off, len = extract_char_unsigned data off len 8 in
 236         let c6, off, len = extract_char_unsigned data off len 8 in
 237         let c7, _, _ = extract_char_unsigned data off len 8 in
 238         let c0 = Int64.of_int c0 in
 239         let c1 = Int64.of_int c1 in
 240         let c2 = Int64.of_int c2 in
 241         let c3 = Int64.of_int c3 in
 242         let c4 = Int64.of_int c4 in
 243         let c5 = Int64.of_int c5 in
 244         let c6 = Int64.of_int c6 in
 245         let c7 = Int64.of_int c7 in
 246         _make_int64_be c0 c1 c2 c3 c4 c5 c6 c7 in
 247       Int64.shift_right_logical word (64 - flen)
 248     ) in
 249   word, off+flen, len-flen
 250
 251 (*----------------------------------------------------------------------*)
 252 (* Constructor functions. *)
 253
 254 module Buffer = struct
 255   type t = {
 256     buf : Buffer.t;
 257     mutable len : int;                  (* Length in bits. *)
 258     (* Last byte in the buffer (if len is not aligned).  We store
 259      * it outside the buffer because buffers aren't mutable.
 260      *)
 261     mutable last : int;
 262   }
 263
 264   let create () =
 265     (* XXX We have almost enough information in the generator to
 266      * choose a good initial size.
 267      *)
 268     { buf = Buffer.create 128; len = 0; last = 0 }
 269
 270   let contents { buf = buf; len = len; last = last } =
 271     let data =
 272       if len land 7 = 0 then
 273         Buffer.contents buf
 274       else
 275         Buffer.contents buf ^ (String.make 1 (Char.chr last)) in
 276     data, 0, len
 277
 278   (* Add exactly 8 bits. *)
 279   let add_byte ({ buf = buf; len = len; last = last } as t) byte =
 280     if byte < 0 || byte > 255 then invalid_arg "Bitmatch.Buffer.add_byte";
 281     let shift = len land 7 in
 282     if shift = 0 then
 283       (* Target buffer is byte-aligned. *)
 284       Buffer.add_char buf (Char.chr byte)
 285     else (
 286       (* Target buffer is unaligned.  'last' is meaningful. *)
 287       let first = byte lsr shift in
 288       let second = (byte lsl (8 - shift)) land 0xff in
 289       Buffer.add_char buf (Char.chr (last lor first));
 290       t.last <- second
 291     );
 292     t.len <- t.len + 8
 293
 294   (* Add exactly 1 bit. *)
 295   let add_bit ({ buf = buf; len = len; last = last } as t) bit =
 296     let shift = 7 - (len land 7) in
 297     if shift > 0 then
 298       (* Somewhere in the middle of 'last'. *)
 299       t.last <- last lor ((if bit then 1 else 0) lsl shift)
 300     else (
 301       (* Just a single spare bit in 'last'. *)
 302       let last = last lor if bit then 1 else 0 in
 303       Buffer.add_char buf (Char.chr last);
 304       t.last <- 0
 305     );
 306     t.len <- len + 1
 307
 308   (* Add a small number of bits (definitely < 8).  This uses a loop
 309    * to call add_bit so it's slow.
 310    *)
 311   let _add_bits t c slen =
 312     if slen < 1 || slen >= 8 then invalid_arg "Bitmatch.Buffer._add_bits";
 313     for i = slen-1 downto 0 do
 314       let bit = c land (1 lsl i) <> 0 in
 315       add_bit t bit
 316     done
 317
 318   let add_bits ({ buf = buf; len = len } as t) str slen =
 319     if slen > 0 then (
 320       if len land 7 = 0 then (
 321         if slen land 7 = 0 then
 322           (* Common case - everything is byte-aligned. *)
 323           Buffer.add_substring buf str 0 (slen lsr 3)
 324         else (
 325           (* Target buffer is aligned.  Copy whole bytes then leave the
 326            * remaining bits in last.
 327            *)
 328           let slenbytes = slen lsr 3 in
 329           if slenbytes > 0 then Buffer.add_substring buf str 0 slenbytes;
 330           t.last <- Char.code str.[slenbytes] lsl (8 - (slen land 7))
 331         );
 332         t.len <- len + slen
 333       ) else (
 334         (* Target buffer is unaligned.  Copy whole bytes using
 335          * add_byte which knows how to deal with an unaligned
 336          * target buffer, then call _add_bits for the remaining < 8 bits.
 337          *
 338          * XXX This is going to be dog-slow.
 339          *)
 340         let slenbytes = slen lsr 3 in
 341         for i = 0 to slenbytes-1 do
 342           let byte = Char.code str.[i] in
 343           add_byte t byte
 344         done;
 345         _add_bits t (Char.code str.[slenbytes]) (slen - (slenbytes lsl 3))
 346       );
 347     )
 348 end
 349
 350 (* Construct a single bit. *)
 351 let construct_bit buf b _ =
 352   Buffer.add_bit buf b
 353
 354 (* Construct a field, flen = [2..8]. *)
 355 let construct_char_unsigned buf v flen exn =
 356   let max_val = 1 lsl flen in
 357   if v < 0 || v >= max_val then raise exn;
 358   if flen = 8 then
 359     Buffer.add_byte buf v
 360   else
 361     Buffer._add_bits buf v flen
 362
 363 (* Generate a mask with the lower 'bits' bits set. *)
 364 let mask64 bits =
 365   if bits < 63 then Int64.pred (Int64.shift_left 1L bits)
 366   else if bits = 63 then Int64.max_int
 367   else if bits = 64 then -1L
 368   else invalid_arg "Bitmatch.mask64"
 369
 370 (* Construct a field of up to 64 bits. *)
 371 let construct_int64_be_unsigned buf v flen exn =
 372   (* Check value is within range. *)
 373   let m = Int64.lognot (mask64 flen) in
 374   if Int64.logand v m <> 0L then raise exn;
 375
 376   (* Add the bytes. *)
 377   let rec loop v flen =
 378     if flen > 8 then (
 379       loop (Int64.shift_right_logical v 8) (flen-8);
 380       let lsb = Int64.to_int (Int64.logand v 0xffL) in
 381       Buffer.add_byte buf lsb
 382     ) else if flen > 0 then (
 383       let lsb = Int64.to_int (Int64.logand v (mask64 flen)) in
 384       Buffer._add_bits buf lsb flen
 385     )
 386   in
 387   loop v flen
 388
 389 (*----------------------------------------------------------------------*)
 390 (* Display functions. *)
 391
 392 let isprint c =
 393   let c = Char.code c in
 394   c >= 32 && c < 127
 395
 396 let hexdump_bitstring chan (data, off, len) =
 397   let count = ref 0 in
 398   let off = ref off in
 399   let len = ref len in
 400   let linelen = ref 0 in
 401   let linechars = String.make 16 ' ' in
 402
 403   fprintf chan "00000000  ";
 404
 405   while !len > 0 do
 406     let bits = min !len 8 in
 407     let byte, off', len' = extract_char_unsigned data !off !len bits in
 408     off := off'; len := len';
 409
 410     let byte = byte lsl (8-bits) in
 411     fprintf chan "%02x " byte;
 412
 413     incr count;
 414     linechars.[!linelen] <-
 415       (let c = Char.chr byte in
 416        if isprint c then c else '.');
 417     incr linelen;
 418     if !linelen = 8 then fprintf chan " ";
 419     if !linelen = 16 then (
 420       fprintf chan " |%s|\n%08x  " linechars !count;
 421       linelen := 0;
 422       for i = 0 to 15 do linechars.[i] <- ' ' done
 423     )
 424   done;
 425
 426   if !linelen > 0 then (
 427     let skip = (16 - !linelen) * 3 + if !linelen < 8 then 1 else 0 in
 428     for i = 0 to skip-1 do fprintf chan " " done;
 429     fprintf chan " |%s|\n" linechars
 430   ) else
 431     fprintf chan "\n"