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