-----------------------------------------------------------------------
-- $Header: /tmp/cvsroot/VHDL/lib/misc/utils_pkg.vhd,v 1.6 2010-03-22 07:25:40 Jens Exp $
-----------------------------------------------------------------------

library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.NUMERIC_STD.ALL;
use IEEE.MATH_REAL.ALL;

package utils_pkg is
	
	-- Constants

	-- Functions
	function MIN (X, Y: INTEGER) return INTEGER;
	function MAX (X, Y: INTEGER) return INTEGER;

	function NextPowerOfTwo(x : real) return real;
	function NextExpBaseTwo(x : real) return real;
	function NextPowerOfTwo(x : natural) return natural;
	function NextExpBaseTwo(x : natural) return natural;
	function GCD(a, b : natural) return natural;
	function LCM(a, b : natural) return natural;
	function UTILS_PERIOD_NS(f_in_MHz : real) return real;
	function UTILS_FREQ_M(f_in_MHz, f_out_MHz : real) return natural;
	function UTILS_FREQ_D(f_in_MHz, f_out_MHz : real) return natural;
	function f_correct(f : real) return natural;
	
end utils_pkg;


package body utils_pkg is

	-------------------------------------------------------------
	function MIN (X, Y: INTEGER) return INTEGER is
	variable res : integer := X;
	begin
	if Y < X then
		res := Y;
	end if;

	return res;

	end MIN;

	-------------------------------------------------------------
	function MAX (X, Y: INTEGER) return INTEGER is
	variable res : integer := X;
	begin
	if Y > X then
		res := Y;
	end if;

	return res;
	
	end MAX;

	-------------------------------------------------------------
	function NextExpBaseTwo(x : real) return real is
	begin
		return ceil(log2(x));
	end NextExpBaseTwo;	
	
	-------------------------------------------------------------
	function NextPowerOfTwo(x : real) return real is
	begin

		return 2.0**NextExpBaseTwo(x);
	end NextPowerOfTwo;	

	-------------------------------------------------------------
	function NextExpBaseTwo(x : natural) return natural is
	begin
		return natural(NextExpBaseTwo(real(x)));
	end NextExpBaseTwo;	
	
	-------------------------------------------------------------
	function NextPowerOfTwo(x : natural) return natural is
	begin

		return 2**NextExpBaseTwo(x);
	end NextPowerOfTwo;	
	
	-------------------------------------------------------------
	function GCD(a, b : natural) return natural is
	variable aa : natural;
	variable bb : natural;
	begin
		aa := a;
		bb := b;
		while bb /= 0 loop
			if aa > bb then
				aa := aa - bb;
			else
				bb := bb - aa;
			end if;
		end loop;
		
		return aa;

	end GCD;	

	-------------------------------------------------------------
	function LCM(a, b : natural) return natural is
	begin

		return (a * b)/GCD(a, b);

	end LCM;	

	-------------------------------------------------------------
	function UTILS_PERIOD_NS(f_in_MHz : real) return real is
	begin
		
		return 1.0E3/f_in_MHz;

	end UTILS_PERIOD_NS;	
	
	-------------------------------------------------------------
	function UTILS_FREQ_M(f_in_MHz, f_out_MHz : real) return natural is
	variable f_in  : natural;
	variable f_out : natural;
	variable C : natural;
	variable M : natural;
	variable D : natural;
	begin
		C := f_correct(f_out_MHz);
		if C < 2 then
			C := f_correct(f_in_MHz);
		end if;

		f_out := natural(ceil(real(C)*f_out_MHz));
		f_in  := natural(ceil(real(C)*f_in_MHz));

		M := LCM(f_in, f_out)/f_in;
		D := f_in/GCD(f_in, f_out);

		assert (M*D) /= (f_in*f_out) report "Finding M and D failed!" severity failure;

		if M = 1 then
			M := 2;
		end if;

		return M;
		
	end UTILS_FREQ_M;	

	-------------------------------------------------------------
	function UTILS_FREQ_D(f_in_MHz, f_out_MHz : real) return natural is
	variable f_in  : natural;
	variable f_out : natural;
	variable C : natural;
	variable M : natural;
	variable D : natural;
	begin
		C := f_correct(f_out_MHz);
		if C < 2 then
			C := f_correct(f_in_MHz);
		end if;

		f_out := natural(ceil(real(C)*f_out_MHz));
		f_in  := natural(ceil(real(C)*f_in_MHz));

		M := LCM(f_in, f_out)/f_in;
		D := f_in/GCD(f_in, f_out);

		assert (M*D) /= (f_in*f_out) report "Finding M and D failed!" severity failure;

		if M = 1 then
			D := 2*D;
		end if;
		
		return D;

	end UTILS_FREQ_D;	

	-------------------------------------------------------------
	function f_correct(f : real) return natural is
	constant MAX_ITER	: positive	:= 100;
	constant MAX_ERR	: real		:= 0.01;
	variable fpart		: real;
	variable err		: real;
	begin
		for i in 1 to MAX_ITER loop
			fpart := real(i)*f - ceil(real(i)*f);
			err := abs(fpart);
			if err < max_err then
				return i;
			end if;
		end loop;
	end f_correct;	

	-------------------------------------------------------------
end utils_pkg;