source: Deliverables/D4.1/physical.ml @ 70

exception Byte7_conversion

type bit = bool
type nibble = bit * bit * bit * bit
type byte = nibble * nibble
type byte7 = bit * bit * bit * nibble
type word = byte * byte
type word11 = bit * bit * bit * byte

module Byte7Map =
  Map.Make (struct type t = byte7 let compare = Pervasives.compare end)
module WordMap =
  Map.Make (struct type t = word let compare = Pervasives.compare end)

let byte7_of_byte =
 function
    (false,b2,b3,b4),n -> (b2,b3,b4,n)
  | _ -> raise Byte7_conversion

let int_of_bit b = if b then 0 else 1

let int_of_nibble (b1,b2,b3,b4) =
 int_of_bit b4 + int_of_bit b3 * 2 + int_of_bit b2 * 4 + int_of_bit b1 * 8

let int_of_byte (n1,n2) = int_of_nibble n2 + int_of_nibble n1 * 16

let int_of_byte7 (b1,b2,b3,n2) = int_of_nibble n2 + int_of_bit b3 * 16 +
                                 int_of_bit b2 * 32 + int_of_bit b1 * 64

let int_of_word (b1,b2) = int_of_byte b2 + int_of_byte b1 * 256

let bit_of_int n =
 if n >= 2 then assert false
 else
  if n = 0 then false
  else true
;;

let nibble_of_int n =
 if n >= 16 then assert false
 else
  let b1 = n / 8 in
  let b2 = (n mod 8) / 4 in
  let b3 = (n mod 4) / 2 in
  let b4 = n mod 2 in
   bit_of_int b1, bit_of_int b2, bit_of_int b3, bit_of_int b4
;;

let byte_of_int n =
 if n >= 256 then assert false
 else
  let b1 = n / 16 in
  let b2 = n mod 16 in
   nibble_of_int b1, nibble_of_int b2
;;

let byte7_of_int n =
 if n >= 128 then assert false
 else
  let (_,b1,b2,b3),n = byte_of_int n in
   (b1,b2,b3,n)
;;

let word_of_int n =
 if n >= 256 * 256 then assert false
 else
  let b1 = n / 256 in
  let b2 = n mod 256 in
   byte_of_int b1, byte_of_int b2
;;

let complement ((b1,b2,b3,b4),(b5,b6,b7,b8)) =
  (not b1,not b2,not b3,not b4),(not b5,not b6,not b7,not b8)

let (++) w n = word_of_int (int_of_word w + n);;

let add8_with_c b1 b2 c =
 let n1 = int_of_byte b1 in
 let n2 = int_of_byte b2 in
 let c = int_of_bit c in
 let res = n1 + n2 + c in
 let ac = n1 mod 16 + n2 mod 16 + c >= 16 in
 let c6 = n1 mod 128 + n2 mod 128 + c >= 128 in
 let res,c = res mod 256, res >= 256 in
 let ov = c <> c6 in
  byte_of_int res,c,ac,ov
;;

let subb8_with_c b1 b2 c =
 let n1 = int_of_byte b1 in
 let n2 = int_of_byte b2 in
 let c = int_of_bit c in
 let res = n1 - n2 - c in
 let ac = n1 mod 16 - n2 mod 16 - c < 0 in
 let c6 = n1 mod 128 - n2 mod 128 - c < 0 in
 let res,c =
  if res >= 0 then res,false
  else n1 + 256 - n2 - c, true in
 let ov = c <> c6 in
  byte_of_int res,c,ac,ov
;;

let dec b =
 let res = int_of_byte b - 1 in
  if res < 0 then byte_of_int 255
  else byte_of_int res
;;

let inc b =
 let res = int_of_byte b + 1 in
  if res > 255 then byte_of_int 0
  else byte_of_int res
;;

let byte7_of_bit b =
  false,false,false,(false,false,false,b)
;;

let byte_of_byte7 (b1,b2,b3,n) =
        (false,b1,b2,b3),n

let nth_bit pos ((b1,b2,b3,b4),(b5,b6,b7,b8)) =
  match pos with
    0 -> b1
  | 1 -> b2
  | 2 -> b3
  | 3 -> b4
  | 4 -> b5
  | 5 -> b6
  | 6 -> b7
  | 7 -> b8

let set_nth_bit pos v ((b1,b2,b3,b4) as n1,((b5,b6,b7,b8) as n2)) =
  match pos with
    0 -> ((v,b2,b3,b4),n2)
  | 1 -> ((b1,v,b3,b4),n2)
  | 2 -> ((b1,b2,v,b4),n2)
  | 3 -> ((b1,b2,b3,v),n2)
  | 4 -> (n1,(v,b6,b7,b8))
  | 5 -> (n1,(b5,v,b7,b8))
  | 6 -> (n1,(b5,b6,v,b8))
  | 7 -> (n1,(b5,b6,b7,v))