let parse_u32 (buf:string) (pos:int) : int32 =
  let u32 = ref Int32.zero in 
    for i = 0 to 3 do 
      u32 := (Int32.logor !u32 
		(Int32.shift_left (Int32.of_int (int_of_char buf.[pos + i]))
		   (8 * i)))
    done;
    !u32  

let parse_u64 (buf:string) (pos:int) : int64 =
  let u64 = ref Int64.zero in 
    for i = 0 to 7 do 
      u64 := (Int64.logor !u64 
		(Int64.shift_left (Int64.of_int (int_of_char buf.[pos + i]))
		   (8 * i)))
    done;
    !u64  

let read_u64 (ic:in_channel) : int64 = 
  let buf = String.create 8 in 
    ignore(really_input ic buf 0 8);
    parse_u64 buf 0

let write_u32 (u32:int32) (oc:out_channel) : unit = 
  for i = 0 to 3 do 
    (output_byte oc (Int32.to_int 
		       (Int32.logand 
			  (Int32.shift_right_logical u32 (8 * i))
			  (Int32.of_int 255))))
  done

let write_u64 (u64:int64) (oc:out_channel) : unit = 
  for i = 0 to 7 do 
    (output_byte oc (Int64.to_int 
		       (Int64.logand 
			  (Int64.shift_right_logical u64 (8 * i))
			  (Int64.of_int 255))))
  done

let u32tostr (u32:int32) :string =
  let cl = ref "" in
  for i = 0 to 3 do 
    cl := Printf.sprintf "%s%c" !cl
	(char_of_int (Int32.to_int 
			(Int32.logand 
			   (Int32.shift_right_logical u32 (8 * i))
			   (Int32.of_int 255))))
  done;
  !cl

let u64tostr (u64:int64) :string =
  let cl = ref "" in
  for i = 0 to 7 do 
    cl := Printf.sprintf "%s%c" !cl
	(char_of_int (Int64.to_int 
			(Int64.logand 
			   (Int64.shift_right_logical u64 (8 * i))
			   (Int64.of_int 255))))
  done;
  !cl

let append_u32 (u32:int32) (oc:Buffer.t) : unit = 
  for i = 0 to 3 do 
    (Buffer.add_char oc (char_of_int (Int32.to_int 
					(Int32.logand 
					   (Int32.shift_right_logical u32 (8 * i))
					   (Int32.of_int 255)))))
  done

let append_u64 (u64:int64) (oc:Buffer.t) : unit = 
  for i = 0 to 7 do 
    (Buffer.add_char oc (char_of_int (Int64.to_int 
					(Int64.logand 
					   (Int64.shift_right_logical u64 (8 * i))
					   (Int64.of_int 255)))))
  done

exception Invalid_opcode of int
exception Invalid_message
exception Unsupported_message

type opCode = 
  | Reserved
  | Rerror
  | Aevent
  | Tversion
  | Rversion
  | Tping
  | Rping
  | Tps
  | Rps
  | Tlaunch
  | Rlaunch
  | Tattach
  | Rattach
  | Tstat
  | Rstat
  | Tcont
  | Rcont
  | Tstop
  | Rstop
  | Tstep
  | Rstep
  | Tsnap
  | Rsnap
  | Tkill
  | Rkill
  | Tdetach
  | Rdetach
  | Ttrace
  | Rtrace
  | Tsettrap
  | Rsettrap
  | Tclrtrap
  | Rclrtrap
  | Tgetctx
  | Rgetctx
  | Tsetctx
  | Rsetctx
  | Tread
  | Rread
  | Twrite
  | Rwrite
  | Tlooksym
  | Rlooksym
  | Tenumsym
  | Renumsym
  | Tlooktype
  | Rlooktype
  | Tenumtype
  | Renumtype
  | Tlookaddr
  | Rlookaddr
  | Tenumloc
  | Renumloc
  | Tenumseg
  | Renumseg
  | Tnames
  | Rnames
  | Tunwind1
  | Runwind1
  | Tlooksrc
  | Rlooksrc
  | Tlookpc
  | Rlookpc

let opcodes = [|
  Reserved;
  Rerror;
  Reserved;
  Aevent;
  Tversion;
  Rversion;
  Tping;
  Rping;
  Tps;
  Rps;
  Tlaunch;
  Rlaunch;
  Tattach;
  Rattach;
  Tstat;
  Rstat;
  Tcont;
  Rcont;
  Tstop;
  Rstop;
  Tstep;
  Rstep;
  Tsnap;
  Rsnap;
  Tkill;
  Rkill;
  Tdetach;
  Rdetach;
  Ttrace;
  Rtrace;
  Tsettrap;
  Rsettrap;
  Tclrtrap;
  Rclrtrap;
  Tgetctx;
  Rgetctx;
  Tsetctx;
  Rsetctx;
  Tread;
  Rread;
  Twrite;
  Rwrite;
  Tlooksym;
  Rlooksym;
  Tenumsym;
  Renumsym;
  Tlooktype;
  Rlooktype;
  Tenumtype;
  Renumtype;
  Tlookaddr;
  Rlookaddr;
  Tenumloc;
  Renumloc;
  Tenumseg;
  Renumseg;
  Tnames;
  Rnames;
  Tunwind1;
  Runwind1;
  Tlooksrc;
  Rlooksrc;
  Tlookpc;
  Rlookpc;
|]

let opcodes_str = [|
  "Reserved";
  "Rerror";
  "Reserved";
  "Aevent";
  "Tversion";
  "Rversion";
  "Tping";
  "Rping";
  "Tps";
  "Rps";
  "Tlaunch";
  "Rlaunch";
  "Tattach";
  "Rattach";
  "Tstat";
  "Rstat";
  "Tcont";
  "Rcont";
  "Tstop";
  "Rstop";
  "Tstep";
  "Rstep";
  "Tsnap";
  "Rsnap";
  "Tkill";
  "Rkill";
  "Tdetach";
  "Rdetach";
  "Ttrace";
  "Rtrace";
  "Tsettrap";
  "Rsettrap";
  "Tclrtrap";
  "Rclrtrap";
  "Tgetctx";
  "Rgetctx";
  "Tsetctx";
  "Rsetctx";
  "Tread";
  "Rread";
  "Twrite";
  "Rwrite";
  "Tlooksym";
  "Rlooksym";
  "Tenumsym";
  "Renumsym";
  "Tlooktype";
  "Rlooktype";
  "Tenumtype";
  "Renumtype";
  "Tlookaddr";
  "Rlookaddr";
  "Tenumloc";
  "Renumloc";
  "Tenumseg";
  "Renumseg";
  "Tnames";
  "Rnames";
  "Tunwind1";
  "Runwind1";
  "Tlooksrc";
  "Rlooksrc";
  "Tlookpc";
  "Rlookpc";
|]

(* index of last occurrence of opcode in above arrays *)
let op2int op : int = 
  let ans = ref (-1) in 
    Array.iteri (fun i el -> if el = op then ans := i) opcodes;
    !ans

let op2str op : string = 
  let idx = op2int op in 
    opcodes_str.(idx)

type msg = {
  (* all messages *)
  op : opCode;
  tag : int32;
  (* some messages *)
  id : int64 option;
  addr: int64 option;
  tid : int64 option;
  cnt : int64 option;
  cid : int64 option;
  flags : int64 option;
  fd : int64 option;
  bytes : string option; 
  nbytes : int64 option;
  raw : string option; 
}

let empty_msg = { op = Reserved ; 
		  tag = 0l;
		  id = None;
		  addr = None;
		  tid = None;
		  cnt = None;
		  cid = None;
		  flags = None;
		  fd = None;
		  bytes = None;
		  nbytes = None;
		  raw = None;
		}

let msg_to_string msg = 
  Printf.sprintf "op=%s tag=%ld addr=%s tid=%s cnt=%s cid=%s flags=%s fd=%s bytes=%s nbytes=%s raw=%s"
    (op2str msg.op) msg.tag
    (match msg.addr with Some a -> Printf.sprintf "0x%Lx" a | None -> "")
    (match msg.tid with Some a -> Printf.sprintf "0x%Lx" a | None -> "")
    (match msg.cnt with Some a -> Printf.sprintf "0x%Lx" a | None -> "")
    (match msg.cid with Some a -> Printf.sprintf "0x%Lx" a | None -> "")
    (match msg.flags with Some a -> Printf.sprintf "0x%Lx" a | None -> "")
    (match msg.fd with Some a -> Printf.sprintf "0x%Lx" a | None -> "")
    (match msg.bytes with Some a -> a | None -> "")
    (match msg.nbytes with Some a -> Printf.sprintf "0x%Lx" a | None -> "")
    (match msg.raw with Some a -> a | None -> "")

let debug_buf buf = 
  for i = 0 to (String.length buf) - 1 do 
    Printf.printf "%.2x " (int_of_char buf.[i]);
  done;
  Printf.printf "\n";
  flush stdout
  
let readmsg (ic:in_channel) : msg = 
  let mlen = Int64.to_int (read_u64 ic) in
  let mdata = String.create mlen in 
  let _ = really_input ic mdata 0 mlen in
  let op = int_of_char mdata.[0] in
  let tag = parse_u32 mdata 1 in 
  let opcode = opcodes.(op) in
  let rmsg = {empty_msg with op = opcode ; tag = tag; nbytes = Some (Int64.of_int(mlen)); raw = Some (mdata);} in
    match opcode with 
	(* op[1] tag[4] bytes[...] *)
      | Rerror | Aevent | Tversion | Rversion 
      | Tping  | Tps    | Rps      | Tlaunch
      | Rstat  | Rstop  | Rstep    | Rgetctx
      | Tlooksym | Rlooksym | Tenumsym | Renumsym
      | Tlooktype | Rlooktype | Tenumtype | Renumtype
      | Tlookaddr | Rlookaddr | Tenumloc | Renumloc
      | Tenumseg | Renumseg
      | Tnames | Rnames | Tunwind1 | Runwind1
      | Tlooksrc | Rlooksrc | Tlookpc | Rlookpc
      | Rread  ->
	  let bytes = String.sub mdata 5 (mlen - 5) in 
	    { rmsg with bytes = Some(bytes);}

	(* op[1] tag[4] cnt[8] *)
      | Rping | Rwrite -> 
	  let cnt = parse_u64 mdata 5 in 
	    { rmsg with cnt = Some(cnt);}

	(* op[1] tag[4] id[8] bytes[...] *)
      | Rlaunch | Rattach | Rsnap | Ttrace | Tsettrap | Tsetctx ->
	  let id = parse_u64 mdata 5 in 
	  let bytes = String.sub mdata 13 (mlen - 13) in 
	    { rmsg with id = Some(id); bytes = Some(bytes);}

	(* op[1] tag[4] id[8] *)
      | Tattach | Tstat   | Tcont   | Tstop | Tstep | Tsnap 
      | Tkill   | Tdetach | Tgetctx  ->
 	  let id = parse_u64 mdata 5 in 
	    { rmsg with id = Some(id); }

	(* op[1] tag[4] tid[8] *)
      | Rsettrap ->
 	  let tid = parse_u64 mdata 5 in 
	    { rmsg with tid = Some(tid); }

	(* op[1] tag[4] id[8] tid[8] *)
      | Tclrtrap ->
 	  let id = parse_u64 mdata 5 in 
	  let tid = parse_u64 mdata 13 in 
	    { rmsg with id = Some(id); tid = Some(tid); }

	(* op[1] tag[4] *)
      | Rcont | Rkill | Rdetach | Rtrace | Rclrtrap 
      | Rsetctx -> rmsg

	 (* op[1] tag[4] id[8] fd[8] offset[8] cnt[8] *)
      | Tread -> 
	  let id = parse_u64 mdata 5 in 
	  let fd = parse_u64 mdata 13 in 
	  let addr = parse_u64 mdata 21 in 
	  let cnt = parse_u64 mdata 29 in 
	    { rmsg with 
		id = Some(id); fd = Some(fd); 
		addr = Some(addr); cnt = Some(cnt) }

	 (* op[1] tag[4] id[8] fd[8] offset[8] bytes[...] *)
      | Twrite -> 
	  let id = parse_u64 mdata 5 in 
	  let fd = parse_u64 mdata 13 in 
	  let addr = parse_u64 mdata 21 in 
	  let bytes = String.sub mdata 29 (mlen - 29) in 
	    { rmsg with 
		id = Some(id); fd = Some(fd); 
		addr = Some(addr); bytes = Some(bytes) }
      | Reserved -> 
	  raise Unsupported_message


let val_of x = match x with Some x' -> x' | None -> raise Invalid_message      

let writemsg (msg:msg) (oc:out_channel) : unit = 
  (match msg.op with 
       (* op[1] tag[4] bytes[...] *)
     | Rerror | Aevent | Tversion | Rversion 
     | Tping  | Tps    | Rps      | Tlaunch
     | Rstat  | Rstop  | Rstep    | Rgetctx
     | Rlooksym | Renumsym
     | Rlooktype | Renumtype
     | Rlookaddr | Renumloc
     | Renumseg
     | Rnames | Tunwind1 | Runwind1
     | Rlooksrc | Rlookpc
     | Rread  ->
	 let bytes = match msg.bytes with 
	     Some x -> x
	   | None -> "" in 
	   write_u64 (Int64.of_int (5 + (String.length bytes))) oc;
	   output_byte oc (op2int msg.op);
	   write_u32 msg.tag oc;
	   output_string oc bytes;
       (* op[1] tag[4] bytes[s] *)
     | Tnames  ->
	 let bytes = match msg.bytes with 
	   Some x -> x
	 | None -> "" in
	 let blen = String.length bytes in
	 write_u64 (Int64.of_int (5 + 8 + blen)) oc;
	 output_byte oc (op2int msg.op);
	 write_u32 msg.tag oc;
	 write_u64 (Int64.of_int blen) oc;
	 output_string oc bytes;
       (* op[1] tag[4] nsid[8] bytes[s] *)
     | Tlooksym  ->
	 let bytes = match msg.bytes with 
	   Some x -> x
	 | None -> "" in 
	 let blen = String.length bytes in
	 write_u64 (Int64.of_int (5 + 16 + blen)) oc;
	 output_byte oc (op2int msg.op);
	 write_u32 msg.tag oc;
	 (match msg.id with
	   Some x -> write_u64 x oc
	 | None -> raise Unsupported_message);
	 write_u64 (Int64.of_int blen) oc;
	 output_string oc bytes;

     | Tlooktype  | Tlookaddr | Tlooksrc | Tlookpc ->
	 let bytes = match msg.bytes with 
	   Some x -> x
	 | None -> "" in 
	 let blen = String.length bytes in
	 write_u64 (Int64.of_int (5 + 8 + blen)) oc;
	 output_byte oc (op2int msg.op);
	 write_u32 msg.tag oc;
	 (match msg.id with
	   Some x -> write_u64 x oc
	 | None -> raise Unsupported_message);
	 output_string oc bytes;

     | Tenumsym | Tenumtype  | Tenumseg ->
	 let id = (match msg.id with 
	   Some x -> x
	 | None -> raise Unsupported_message) in
	 write_u64 13L oc;
	 output_byte oc (op2int msg.op);
	 write_u32 msg.tag oc;
	 write_u64 id oc;

     (* op[1] tag[4] cnt[8] *)
     | Rping | Rwrite -> 
	 write_u64 13L oc;
	 output_byte oc (op2int msg.op);
	 write_u32 msg.tag oc;
	 write_u64 (val_of msg.cnt) oc;
	 
     (* op[1] tag[4] id[8] bytes[...] *)
     | Rlaunch | Rattach | Ttrace | Tsettrap | Tsetctx | Tenumloc ->
	 let bytes = match msg.bytes with 
	     Some x -> x
	   | None -> "" in 
	   write_u64 (Int64.of_int (13 + (String.length bytes))) oc;
	   output_byte oc (op2int msg.op);
	   write_u32 msg.tag oc;
	   write_u64 (val_of msg.id) oc;
	   output_string oc bytes;
	   
     (* op[1] tag[4] id[8] *)
     | Tattach | Tstat   | Tcont   | Tstop | Tstep | Tsnap 
     | Tkill   | Tdetach | Tgetctx  ->
	 write_u64 13L oc;
	 output_byte oc (op2int msg.op);
	 write_u32 msg.tag oc;
	 write_u64 (val_of msg.id) oc;
	 
     (* op[1] tag[4] tid[8] *)
     | Rsettrap ->
	 write_u64 13L oc;
	 output_byte oc (op2int msg.op);
	 write_u32 msg.tag oc;
	 write_u64 (val_of msg.tid) oc;
	 
     (* op[1] tag[4] id[8] tid[8] *)
     | Tclrtrap ->
	 write_u64 21L oc;
	 output_byte oc (op2int msg.op);
	 write_u32 msg.tag oc;
	 write_u64 (val_of msg.id) oc;
	 write_u64 (val_of msg.tid) oc;

     (* Rsnap[1] tag[4] . cid[8] bytes[...] *)
     | Rsnap ->
	 let bytes = match msg.bytes with 
	     Some x -> x
	   | None -> "" in 
	   write_u64 (Int64.of_int (13 + (String.length bytes))) oc;
	   output_byte oc (op2int msg.op);
	   write_u32 msg.tag oc;
	   write_u64 (val_of msg.cid) oc;
	   output_string oc bytes;
	 
     (* op[1] tag[4] *)
     | Rcont | Rkill | Rdetach | Rtrace | Rclrtrap 
     | Rsetctx ->
	 write_u64 5L oc;
	 output_byte oc (op2int msg.op);
	 write_u32 msg.tag oc;
	 
     (* op[1] tag[4] id[8] fd[8] offset[8] cnt[8] *)
     | Tread -> 
	 write_u64 37L oc;
	 output_byte oc (op2int msg.op);
	 write_u32 msg.tag oc;
	 write_u64 (val_of msg.id) oc;
	 write_u64 (val_of msg.fd) oc;
	 write_u64 (val_of msg.addr) oc;
	 write_u64 (val_of msg.cnt) oc;
	 
     (* op[1] tag[4] id[8] fd[8] offset[8] bytes[...] *)
     | Twrite -> 
	 let bytes = match msg.bytes with 
	     Some x -> x
	   | None -> "" in 
	   write_u64 (Int64.of_int (29 + (String.length bytes))) oc;
	   output_byte oc (op2int msg.op);
	   write_u32 msg.tag oc;
	   write_u64 (val_of msg.id) oc;
	   write_u64 (val_of msg.fd) oc;
	   write_u64 (val_of msg.addr) oc;
	   output_string oc bytes;	
	   
     | Reserved -> 
	 raise Unsupported_message);
  flush oc