#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <stdint.h>
#include <math.h>

typedef unsigned int CARD32;
#define CARD32 CARD32

typedef unsigned long CARD64;
#define CARD64 CARD64

static uint32_t RADEONDiv(CARD64 n, uint32_t d)
{
    return (n + (d / 2)) / d;
}

#define RADEON_PLL_USE_BIOS_DIVS   (1 << 0)
#define RADEON_PLL_NO_ODD_POST_DIV (1 << 1)
#define RADEON_PLL_USE_REF_DIV     (1 << 2)
#define RADEON_PLL_LEGACY          (1 << 3)
#define RADEON_PLL_PREFER_LOW_REF_DIV (1 << 4)
#define RADEON_PLL_PREFER_HIGH_REF_DIV (1 << 5)
#define RADEON_PLL_PREFER_LOW_FB_DIV (1 << 6)
#define RADEON_PLL_PREFER_HIGH_FB_DIV (1 << 7)
#define RADEON_PLL_PREFER_LOW_POST_DIV (1 << 8)
#define RADEON_PLL_PREFER_HIGH_POST_DIV (1 << 9)

typedef struct {
    uint16_t          reference_freq;
    uint16_t          reference_div;
    uint32_t          pll_in_min;
    uint32_t          pll_in_max;
    uint32_t          pll_out_min;
    uint32_t          pll_out_max;
    uint16_t          xclk;

    uint32_t          min_ref_div;
    uint32_t          max_ref_div;
    uint32_t          min_post_div;
    uint32_t          max_post_div;
    uint32_t          min_feedback_div;
    uint32_t          max_feedback_div;
    uint32_t          min_frac_feedback_div;
    uint32_t          max_frac_feedback_div;
    uint32_t          best_vco;
} RADEONPLLRec, *RADEONPLLPtr;


void
RADEONComputePLL(RADEONPLLPtr pll,
		 unsigned long freq,
		 int flags)
{
    uint32_t min_ref_div = pll->min_ref_div;
    uint32_t max_ref_div = pll->max_ref_div;
    uint32_t best_vco = pll->best_vco;
    uint32_t best_post_div = 1;
    uint32_t best_ref_div = 1;
    uint32_t best_feedback_div = 1;
    uint32_t best_frac_feedback_div = 0;
    uint32_t best_freq = -1;
    uint32_t best_error = 0xffffffff;
    uint32_t best_vco_diff = 1;
    uint32_t post_div;

    freq = freq * 1000;

    printf("freq: %lu\n", freq);

    if (flags & RADEON_PLL_USE_REF_DIV)
	min_ref_div = max_ref_div = pll->reference_div;
    else {
	while (min_ref_div < max_ref_div-1) {
	    uint32_t mid=(min_ref_div+max_ref_div)/2;
	    uint32_t pll_in = pll->reference_freq / mid;
	    if (pll_in < pll->pll_in_min)
		max_ref_div = mid;
	    else if (pll_in > pll->pll_in_max)
		min_ref_div = mid;
	    else break;
	}
    }

    for (post_div = pll->min_post_div; post_div <= pll->max_post_div; ++post_div) {
	uint32_t ref_div;

	if ((flags & RADEON_PLL_NO_ODD_POST_DIV) && (post_div & 1))
	    continue;

	/* legacy radeons only have a few post_divs */
	if (flags & RADEON_PLL_LEGACY) {
	    if ((post_div == 5) ||
		(post_div == 7) ||
		(post_div == 9) ||
		(post_div == 10) ||
		(post_div == 11))
		continue;
	}

	for (ref_div = min_ref_div; ref_div <= max_ref_div; ++ref_div) {
	    uint32_t feedback_div, current_freq, error, vco_diff;
	    uint32_t pll_in = pll->reference_freq / ref_div;
	    uint32_t min_feed_div = pll->min_feedback_div;
	    uint32_t max_feed_div = pll->max_feedback_div+1;

	    if (pll_in < pll->pll_in_min || pll_in > pll->pll_in_max)
		continue;

	    while (min_feed_div < max_feed_div) {
		uint32_t vco;
		uint32_t frac_fb_div;
		CARD64 tmp;

		feedback_div = (min_feed_div+max_feed_div)/2;

		tmp = pll->reference_freq * feedback_div;
		vco = RADEONDiv(tmp, ref_div);

		if (vco < pll->pll_out_min) {
		    min_feed_div = feedback_div+1;
		    continue;
		} else if(vco > pll->pll_out_max) {
		    max_feed_div = feedback_div;
		    continue;
		}

		for (frac_fb_div = pll->min_frac_feedback_div; frac_fb_div <= pll->max_frac_feedback_div; frac_fb_div++) {

		    tmp = pll->reference_freq * 10000 * feedback_div;
		    tmp += pll->reference_freq * 1000 * frac_fb_div;
		    current_freq = RADEONDiv(tmp, ref_div * post_div);

		    error = abs(current_freq - freq);
		    vco_diff = abs(vco - best_vco);

		    if ((best_vco == 0 && error < best_error) ||
			(best_vco != 0 &&
			 (error < best_error - 100 ||
			  (abs(error - best_error) < 100 && vco_diff < best_vco_diff )))) {
			best_post_div = post_div;
			best_ref_div = ref_div;
			best_feedback_div = feedback_div;
			best_frac_feedback_div = frac_fb_div;
			best_freq = current_freq;
			best_error = error;
			best_vco_diff = vco_diff;
		    } else if (current_freq == freq) {
			if (best_freq == -1) {
			    best_post_div = post_div;
			    best_ref_div = ref_div;
			    best_feedback_div = feedback_div;
			    best_frac_feedback_div = frac_fb_div;
			    best_freq = current_freq;
			    best_error = error;
			    best_vco_diff = vco_diff;
			} else if (((flags & RADEON_PLL_PREFER_LOW_REF_DIV) && (ref_div < best_ref_div)) ||
				   ((flags & RADEON_PLL_PREFER_HIGH_REF_DIV) && (ref_div > best_ref_div)) ||
				   ((flags & RADEON_PLL_PREFER_LOW_FB_DIV) && (feedback_div < best_feedback_div)) ||
				   ((flags & RADEON_PLL_PREFER_HIGH_FB_DIV) && (feedback_div > best_feedback_div)) ||
				   ((flags & RADEON_PLL_PREFER_LOW_POST_DIV) && (post_div < best_post_div)) ||
				   ((flags & RADEON_PLL_PREFER_HIGH_POST_DIV) && (post_div > best_post_div))) {
			    best_post_div = post_div;
			    best_ref_div = ref_div;
			    best_feedback_div = feedback_div;
			    best_frac_feedback_div = frac_fb_div;
			    best_freq = current_freq;
			    best_error = error;
			    best_vco_diff = vco_diff;

			}
		    } else if (current_freq == best_freq) {
			if (((flags & RADEON_PLL_PREFER_LOW_REF_DIV) && (ref_div < best_ref_div)) ||
			    ((flags & RADEON_PLL_PREFER_HIGH_REF_DIV) && (ref_div > best_ref_div)) ||
			    ((flags & RADEON_PLL_PREFER_LOW_FB_DIV) && (feedback_div < best_feedback_div)) ||
			    ((flags & RADEON_PLL_PREFER_HIGH_FB_DIV) && (feedback_div > best_feedback_div)) ||
			    ((flags & RADEON_PLL_PREFER_LOW_POST_DIV) && (post_div < best_post_div)) ||
			    ((flags & RADEON_PLL_PREFER_HIGH_POST_DIV) && (post_div > best_post_div))) {
			    best_post_div = post_div;
			    best_ref_div = ref_div;
			    best_feedback_div = feedback_div;
			    best_frac_feedback_div = frac_fb_div;
			    best_freq = current_freq;
			    best_error = error;
			    best_vco_diff = vco_diff;
			}
		    }
		}
		if (current_freq < freq)
		    min_feed_div = feedback_div+1;
		else
		    max_feed_div = feedback_div;
	    }
	}
    }

    printf("best_freq: %u\n", (unsigned int)best_freq);
    printf("best_feedback_div: %u.%u\n", (unsigned int)best_feedback_div,
	   (unsigned int)best_frac_feedback_div);
    printf("best_ref_div: %u\n", (unsigned int)best_ref_div);
    printf("best_post_div: %u\n", (unsigned int)best_post_div);

}

void
RADEONComputePLL_new(RADEONPLLPtr pll,
		     unsigned long freq,
		     int flags)
{
    float m, n, p, f_vco, f_pclk, error, best_freq;
    float pll_out_max = pll->pll_out_max;
    float pll_out_min = pll->pll_out_min;
    float reference_freq = pll->reference_freq;
    float pll_in_max = pll->pll_in_max;
    float pll_in_min = pll->pll_in_min;
    float ffreq = freq / 10;


    printf("freq: %lu\n", freq);

    //1 - max P
    p = floor(pll_out_max / ffreq);

    //2 - min m
    m = ceil(reference_freq / pll_in_max);

    //3 - n
step3:
    n = (ffreq / reference_freq) * m * p;

    //4
    f_vco = (n / m) * reference_freq;

    //5
    f_pclk = f_vco / p;

    // 6
    error = (abs(f_pclk - ffreq) / f_pclk) * 100;

    //printf("error: %f\n", error);
    //printf("%d %d\n", f_pclk, freq);

    best_freq = reference_freq * (n / (m * p));

    printf("f: %f m: %f n: %f p: %f\n", best_freq, m, n, p);

    // 0.25%
    if (error > 0.25)
	goto step7;
    else
	goto done;

    // 7
step7:
    m++;
    if ((reference_freq / m) >= pll_in_min)
	goto step3;
    else
	goto step8;

    // 8
step8:
    m = ceil(reference_freq / pll_in_max);
    p--;
    if ((p * ffreq) >= pll_out_min)
	goto step3;
    else
	return;

done:

    printf("best_freq: %f\n", best_freq);
    printf("n: %f\n", n);
    printf("m: %f\n", m);
    printf("p: %f\n", p);

}

int main(int argc,char *argv[])
{
    CARD32 dotclock;
    RADEONPLLRec pll;
    //int flags = RADEON_PLL_PREFER_HIGH_FB_DIV;
    int flags = RADEON_PLL_PREFER_LOW_REF_DIV;

    if (argc == 1) {
	printf("test_pll dotclock\n");
	return 1;
    }
#if 1
    pll.reference_freq = 2700;
    pll.reference_div = 12;
    pll.pll_out_min = 64800;
    pll.pll_out_max = 110000;
    pll.pll_in_min = 100;
    pll.pll_in_max = 1350;
    pll.min_post_div = 2;
    pll.max_post_div = 0x7f;
#else
    pll.reference_freq = 2700;
    pll.reference_div = 12;
    pll.pll_out_min = 20000;
    pll.pll_out_max = 50000;
    pll.pll_in_min = 100;
    pll.pll_in_max = 1350;
    pll.min_post_div = 1;
    pll.max_post_div = 12;
#endif
#if 0
    //rs690
    pll.reference_freq = 1432;
    pll.reference_div = 13;
    pll.pll_out_min = 64800;
    pll.pll_out_max = 120000;
    pll.pll_in_min = 100;
    pll.pll_in_max = 1350;
    pll.min_post_div = 2;
    pll.max_post_div = 0x7f;
    //#else
    //rv250
    pll.reference_freq = 2700;
    pll.reference_div = 12;
    pll.pll_out_min = 20000;
    pll.pll_out_max = 35000;
    pll.pll_in_min = 40;
    pll.pll_in_max = 3000;
    pll.min_post_div = 1;
    pll.max_post_div = 12;//16;
#endif

    pll.min_ref_div = 2;
    pll.max_ref_div = 0x3ff;
    pll.min_feedback_div = 4;
    pll.max_feedback_div = 0x7ff;
    pll.min_frac_feedback_div = 0;
    pll.max_frac_feedback_div = 9;
    pll.best_vco = 0;

    dotclock = strtoul(argv[1], NULL, 0);
    printf("dotclock: %d\n", dotclock);

    flags = 0;
    RADEONComputePLL(&pll, dotclock, flags);
    RADEONComputePLL_new(&pll, dotclock, flags);

    return 0;
}