+
+let extract_int64_ne_unsigned =
+ if nativeendian = BigEndian
+ then extract_int64_be_unsigned
+ else extract_int64_le_unsigned
+
+let extract_int64_ee_unsigned = function
+ | BigEndian -> extract_int64_be_unsigned
+ | LittleEndian -> extract_int64_le_unsigned
+ | NativeEndian -> extract_int64_ne_unsigned
+
+(*----------------------------------------------------------------------*)
+(* Constructor functions. *)
+
+module Buffer = struct
+ type t = {
+ buf : Buffer.t;
+ mutable len : int; (* Length in bits. *)
+ (* Last byte in the buffer (if len is not aligned). We store
+ * it outside the buffer because buffers aren't mutable.
+ *)
+ mutable last : int;
+ }
+
+ let create () =
+ (* XXX We have almost enough information in the generator to
+ * choose a good initial size.
+ *)
+ { buf = Buffer.create 128; len = 0; last = 0 }
+
+ let contents { buf = buf; len = len; last = last } =
+ let data =
+ if len land 7 = 0 then
+ Buffer.contents buf
+ else
+ Buffer.contents buf ^ (String.make 1 (Char.chr last)) in
+ data, 0, len
+
+ (* Add exactly 8 bits. *)
+ let add_byte ({ buf = buf; len = len; last = last } as t) byte =
+ if byte < 0 || byte > 255 then invalid_arg "Bitmatch.Buffer.add_byte";
+ let shift = len land 7 in
+ if shift = 0 then
+ (* Target buffer is byte-aligned. *)
+ Buffer.add_char buf (Char.chr byte)
+ else (
+ (* Target buffer is unaligned. 'last' is meaningful. *)
+ let first = byte lsr shift in
+ let second = (byte lsl (8 - shift)) land 0xff in
+ Buffer.add_char buf (Char.chr (last lor first));
+ t.last <- second
+ );
+ t.len <- t.len + 8
+
+ (* Add exactly 1 bit. *)
+ let add_bit ({ buf = buf; len = len; last = last } as t) bit =
+ let shift = 7 - (len land 7) in
+ if shift > 0 then
+ (* Somewhere in the middle of 'last'. *)
+ t.last <- last lor ((if bit then 1 else 0) lsl shift)
+ else (
+ (* Just a single spare bit in 'last'. *)
+ let last = last lor if bit then 1 else 0 in
+ Buffer.add_char buf (Char.chr last);
+ t.last <- 0
+ );
+ t.len <- len + 1
+
+ (* Add a small number of bits (definitely < 8). This uses a loop
+ * to call add_bit so it's slow.
+ *)
+ let _add_bits t c slen =
+ if slen < 1 || slen >= 8 then invalid_arg "Bitmatch.Buffer._add_bits";
+ for i = slen-1 downto 0 do
+ let bit = c land (1 lsl i) <> 0 in
+ add_bit t bit
+ done
+
+ let add_bits ({ buf = buf; len = len } as t) str slen =
+ if slen > 0 then (
+ if len land 7 = 0 then (
+ if slen land 7 = 0 then
+ (* Common case - everything is byte-aligned. *)
+ Buffer.add_substring buf str 0 (slen lsr 3)
+ else (
+ (* Target buffer is aligned. Copy whole bytes then leave the
+ * remaining bits in last.
+ *)
+ let slenbytes = slen lsr 3 in
+ if slenbytes > 0 then Buffer.add_substring buf str 0 slenbytes;
+ let last = Char.code str.[slenbytes] in (* last char *)
+ let mask = 0xff lsl (8 - (slen land 7)) in
+ t.last <- last land mask
+ );
+ t.len <- len + slen
+ ) else (
+ (* Target buffer is unaligned. Copy whole bytes using
+ * add_byte which knows how to deal with an unaligned
+ * target buffer, then call add_bit for the remaining < 8 bits.
+ *
+ * XXX This is going to be dog-slow.
+ *)
+ let slenbytes = slen lsr 3 in
+ for i = 0 to slenbytes-1 do
+ let byte = Char.code str.[i] in
+ add_byte t byte
+ done;
+ let bitsleft = slen - (slenbytes lsl 3) in
+ if bitsleft > 0 then (
+ let c = Char.code str.[slenbytes] in
+ for i = 0 to bitsleft - 1 do
+ let bit = c land (0x80 lsr i) <> 0 in
+ add_bit t bit
+ done
+ )
+ );
+ )
+end
+
+(* Construct a single bit. *)
+let construct_bit buf b _ _ =
+ Buffer.add_bit buf b
+
+(* Construct a field, flen = [2..8]. *)
+let construct_char_unsigned buf v flen exn =
+ let max_val = 1 lsl flen in
+ if v < 0 || v >= max_val then raise exn;
+ if flen = 8 then
+ Buffer.add_byte buf v
+ else
+ Buffer._add_bits buf v flen
+
+(* Construct a field of up to 31 bits. *)
+let construct_int_be_unsigned buf v flen exn =
+ (* Check value is within range. *)
+ if not (I.range_unsigned v flen) then raise exn;
+ (* Add the bytes. *)
+ I.map_bytes_be (Buffer._add_bits buf) (Buffer.add_byte buf) v flen
+
+let construct_int_ne_unsigned =
+ if nativeendian = BigEndian
+ then construct_int_be_unsigned
+ else (*construct_int_le_unsigned*)
+ fun _ _ _ _ -> failwith "construct_int_le_unsigned"
+
+let construct_int_ee_unsigned = function
+ | BigEndian -> construct_int_be_unsigned
+ | LittleEndian -> (*construct_int_le_unsigned*)
+ (fun _ _ _ _ -> failwith "construct_int_le_unsigned")
+ | NativeEndian -> construct_int_ne_unsigned
+
+(* Construct a field of exactly 32 bits. *)
+let construct_int32_be_unsigned buf v flen _ =
+ Buffer.add_byte buf
+ (Int32.to_int (Int32.shift_right_logical v 24));
+ Buffer.add_byte buf
+ (Int32.to_int ((Int32.logand (Int32.shift_right_logical v 16) 0xff_l)));
+ Buffer.add_byte buf
+ (Int32.to_int ((Int32.logand (Int32.shift_right_logical v 8) 0xff_l)));
+ Buffer.add_byte buf
+ (Int32.to_int (Int32.logand v 0xff_l))
+
+let construct_int32_le_unsigned buf v flen _ =
+ Buffer.add_byte buf
+ (Int32.to_int (Int32.logand v 0xff_l));
+ Buffer.add_byte buf
+ (Int32.to_int ((Int32.logand (Int32.shift_right_logical v 8) 0xff_l)));
+ Buffer.add_byte buf
+ (Int32.to_int ((Int32.logand (Int32.shift_right_logical v 16) 0xff_l)));
+ Buffer.add_byte buf
+ (Int32.to_int (Int32.shift_right_logical v 24))
+
+let construct_int32_ne_unsigned =
+ if nativeendian = BigEndian
+ then construct_int32_be_unsigned
+ else construct_int32_le_unsigned
+
+let construct_int32_ee_unsigned = function
+ | BigEndian -> construct_int32_be_unsigned
+ | LittleEndian -> construct_int32_le_unsigned
+ | NativeEndian -> construct_int32_ne_unsigned
+
+(* Construct a field of up to 64 bits. *)
+let construct_int64_be_unsigned buf v flen exn =
+ (* Check value is within range. *)
+ if not (I64.range_unsigned v flen) then raise exn;
+ (* Add the bytes. *)
+ I64.map_bytes_be (Buffer._add_bits buf) (Buffer.add_byte buf) v flen
+
+let construct_int64_ne_unsigned =
+ if nativeendian = BigEndian
+ then construct_int64_be_unsigned
+ else (*construct_int64_le_unsigned*)
+ fun _ _ _ _ -> failwith "construct_int64_le_unsigned"
+
+let construct_int64_ee_unsigned = function
+ | BigEndian -> construct_int64_be_unsigned
+ | LittleEndian -> (*construct_int64_le_unsigned*)
+ (fun _ _ _ _ -> failwith "construct_int64_le_unsigned")
+ | NativeEndian -> construct_int64_ne_unsigned
+
+(* Construct from a string of bytes, exact multiple of 8 bits
+ * in length of course.
+ *)
+let construct_string buf str =
+ let len = String.length str in
+ Buffer.add_bits buf str (len lsl 3)
+
+(*----------------------------------------------------------------------*)
+(* Extract a string from a bitstring. *)
+
+let string_of_bitstring (data, off, len) =
+ if off land 7 = 0 && len land 7 = 0 then
+ (* Easy case: everything is byte-aligned. *)
+ String.sub data (off lsr 3) (len lsr 3)
+ else (
+ (* Bit-twiddling case. *)
+ let strlen = (len + 7) lsr 3 in
+ let str = String.make strlen '\000' in
+ let rec loop data off len i =
+ if len >= 8 then (
+ let c, off, len = extract_char_unsigned data off len 8 in
+ str.[i] <- Char.chr c;
+ loop data off len (i+1)
+ ) else if len > 0 then (
+ let c, _, _ = extract_char_unsigned data off len len in
+ str.[i] <- Char.chr (c lsl (8-len))
+ )
+ in
+ loop data off len 0;
+ str
+ )
+
+(* To channel. *)
+
+let bitstring_to_chan ((data, off, len) as bits) chan =
+ (* Fail if the bitstring length isn't a multiple of 8. *)
+ if len land 7 <> 0 then invalid_arg "bitstring_to_chan";
+
+ if off land 7 = 0 then
+ (* Easy case: string is byte-aligned. *)
+ output chan data (off lsr 3) (len lsr 3)
+ else (
+ (* Bit-twiddling case: reuse string_of_bitstring *)
+ let str = string_of_bitstring bits in
+ output_string chan str
+ )
+
+let bitstring_to_file bits filename =
+ let chan = open_out_bin filename in
+ try
+ bitstring_to_chan bits chan;
+ close_out chan
+ with exn ->
+ close_out chan;
+ raise exn
+
+(*----------------------------------------------------------------------*)
+(* Display functions. *)
+
+let isprint c =
+ let c = Char.code c in
+ c >= 32 && c < 127
+
+let hexdump_bitstring chan (data, off, len) =
+ let count = ref 0 in
+ let off = ref off in
+ let len = ref len in
+ let linelen = ref 0 in
+ let linechars = String.make 16 ' ' in
+
+ fprintf chan "00000000 ";
+
+ while !len > 0 do
+ let bits = min !len 8 in
+ let byte, off', len' = extract_char_unsigned data !off !len bits in
+ off := off'; len := len';
+
+ let byte = byte lsl (8-bits) in
+ fprintf chan "%02x " byte;
+
+ incr count;
+ linechars.[!linelen] <-
+ (let c = Char.chr byte in
+ if isprint c then c else '.');
+ incr linelen;
+ if !linelen = 8 then fprintf chan " ";
+ if !linelen = 16 then (
+ fprintf chan " |%s|\n%08x " linechars !count;
+ linelen := 0;
+ for i = 0 to 15 do linechars.[i] <- ' ' done
+ )
+ done;
+
+ if !linelen > 0 then (
+ let skip = (16 - !linelen) * 3 + if !linelen < 8 then 1 else 0 in
+ for i = 0 to skip-1 do fprintf chan " " done;
+ fprintf chan " |%s|\n%!" linechars
+ ) else
+ fprintf chan "\n%!"