[deliverable/linux.git] / drivers / video / matrox / g450_pll.c

/*
 *
 * Hardware accelerated Matrox PCI cards - G450/G550 PLL control.
 *
 * (c) 2001-2002 Petr Vandrovec <vandrove@vc.cvut.cz>
 *
 * Portions Copyright (c) 2001 Matrox Graphics Inc.
 *
 * Version: 1.64 2002/06/10
 *
 * This file is subject to the terms and conditions of the GNU General Public
 * License. See the file COPYING in the main directory of this archive for
 * more details.
 *
 */

#include "g450_pll.h"
#include "matroxfb_DAC1064.h"

static inline unsigned int g450_vco2f(unsigned char p, unsigned int fvco) {
	return (p & 0x40) ? fvco : fvco >> ((p & 3) + 1);
}

static inline unsigned int g450_f2vco(unsigned char p, unsigned int fin) {
	return (p & 0x40) ? fin : fin << ((p & 3) + 1);
}

static unsigned int g450_mnp2vco(const struct matrox_fb_info *minfo,
				 unsigned int mnp)
{
	unsigned int m, n;

	m = ((mnp >> 16) & 0x0FF) + 1;
	n = ((mnp >>  7) & 0x1FE) + 4;
	return (minfo->features.pll.ref_freq * n + (m >> 1)) / m;
}

unsigned int g450_mnp2f(const struct matrox_fb_info *minfo, unsigned int mnp)
{
	return g450_vco2f(mnp, g450_mnp2vco(minfo, mnp));
}

static inline unsigned int pll_freq_delta(unsigned int f1, unsigned int f2) {
	if (f2 < f1) {
    		f2 = f1 - f2;
	} else {
		f2 = f2 - f1;
	}
	return f2;
}

#define NO_MORE_MNP	0x01FFFFFF
#define G450_MNP_FREQBITS	(0xFFFFFF43)	/* do not mask high byte so we'll catch NO_MORE_MNP */

static unsigned int g450_nextpll(const struct matrox_fb_info *minfo,
				 const struct matrox_pll_limits *pi,
				 unsigned int *fvco, unsigned int mnp)
{
	unsigned int m, n, p;
	unsigned int tvco = *fvco;

	m = (mnp >> 16) & 0xFF;
	p = mnp & 0xFF;

	do {
		if (m == 0 || m == 0xFF) {
			if (m == 0) {
				if (p & 0x40) {
					return NO_MORE_MNP;
				}
			        if (p & 3) {
					p--;
				} else {
					p = 0x40;
				}
				tvco >>= 1;
				if (tvco < pi->vcomin) {
					return NO_MORE_MNP;
				}
				*fvco = tvco;
			}

			p &= 0x43;
			if (tvco < 550000) {
/*				p |= 0x00; */
			} else if (tvco < 700000) {
				p |= 0x08;
			} else if (tvco < 1000000) {
				p |= 0x10;
			} else if (tvco < 1150000) {
				p |= 0x18;
			} else {
				p |= 0x20;
			}
			m = 9;
		} else {
			m--;
		}
		n = ((tvco * (m+1) + minfo->features.pll.ref_freq) / (minfo->features.pll.ref_freq * 2)) - 2;
	} while (n < 0x03 || n > 0x7A);
	return (m << 16) | (n << 8) | p;
}

static unsigned int g450_firstpll(const struct matrox_fb_info *minfo,
				  const struct matrox_pll_limits *pi,
				  unsigned int *vco, unsigned int fout)
{
	unsigned int p;
	unsigned int vcomax;

	vcomax = pi->vcomax;
	if (fout > (vcomax / 2)) {
		if (fout > vcomax) {
			*vco = vcomax;
		} else {
			*vco = fout;
		}
		p = 0x40;
	} else {
		unsigned int tvco;

		p = 3;
		tvco = g450_f2vco(p, fout);
		while (p && (tvco > vcomax)) {
			p--;
			tvco >>= 1;
		}
		if (tvco < pi->vcomin) {
			tvco = pi->vcomin;
		}
		*vco = tvco;
	}
	return g450_nextpll(minfo, pi, vco, 0xFF0000 | p);
}

static inline unsigned int g450_setpll(const struct matrox_fb_info *minfo,
				       unsigned int mnp, unsigned int pll)
{
	switch (pll) {
		case M_PIXEL_PLL_A:
			matroxfb_DAC_out(minfo, M1064_XPIXPLLAM, mnp >> 16);
			matroxfb_DAC_out(minfo, M1064_XPIXPLLAN, mnp >> 8);
			matroxfb_DAC_out(minfo, M1064_XPIXPLLAP, mnp);
			return M1064_XPIXPLLSTAT;

		case M_PIXEL_PLL_B:
			matroxfb_DAC_out(minfo, M1064_XPIXPLLBM, mnp >> 16);
			matroxfb_DAC_out(minfo, M1064_XPIXPLLBN, mnp >> 8);
			matroxfb_DAC_out(minfo, M1064_XPIXPLLBP, mnp);
			return M1064_XPIXPLLSTAT;

		case M_PIXEL_PLL_C:
			matroxfb_DAC_out(minfo, M1064_XPIXPLLCM, mnp >> 16);
			matroxfb_DAC_out(minfo, M1064_XPIXPLLCN, mnp >> 8);
			matroxfb_DAC_out(minfo, M1064_XPIXPLLCP, mnp);
			return M1064_XPIXPLLSTAT;

		case M_SYSTEM_PLL:
			matroxfb_DAC_out(minfo, DAC1064_XSYSPLLM, mnp >> 16);
			matroxfb_DAC_out(minfo, DAC1064_XSYSPLLN, mnp >> 8);
			matroxfb_DAC_out(minfo, DAC1064_XSYSPLLP, mnp);
			return DAC1064_XSYSPLLSTAT;

		case M_VIDEO_PLL:
			matroxfb_DAC_out(minfo, M1064_XVIDPLLM, mnp >> 16);
			matroxfb_DAC_out(minfo, M1064_XVIDPLLN, mnp >> 8);
			matroxfb_DAC_out(minfo, M1064_XVIDPLLP, mnp);
			return M1064_XVIDPLLSTAT;
	}
	return 0;
}

static inline unsigned int g450_cmppll(const struct matrox_fb_info *minfo,
				       unsigned int mnp, unsigned int pll)
{
	unsigned char m = mnp >> 16;
	unsigned char n = mnp >> 8;
	unsigned char p = mnp;

	switch (pll) {
		case M_PIXEL_PLL_A:
			return (matroxfb_DAC_in(minfo, M1064_XPIXPLLAM) != m ||
				matroxfb_DAC_in(minfo, M1064_XPIXPLLAN) != n ||
				matroxfb_DAC_in(minfo, M1064_XPIXPLLAP) != p);

		case M_PIXEL_PLL_B:
			return (matroxfb_DAC_in(minfo, M1064_XPIXPLLBM) != m ||
				matroxfb_DAC_in(minfo, M1064_XPIXPLLBN) != n ||
				matroxfb_DAC_in(minfo, M1064_XPIXPLLBP) != p);

		case M_PIXEL_PLL_C:
			return (matroxfb_DAC_in(minfo, M1064_XPIXPLLCM) != m ||
				matroxfb_DAC_in(minfo, M1064_XPIXPLLCN) != n ||
				matroxfb_DAC_in(minfo, M1064_XPIXPLLCP) != p);

		case M_SYSTEM_PLL:
			return (matroxfb_DAC_in(minfo, DAC1064_XSYSPLLM) != m ||
				matroxfb_DAC_in(minfo, DAC1064_XSYSPLLN) != n ||
				matroxfb_DAC_in(minfo, DAC1064_XSYSPLLP) != p);

		case M_VIDEO_PLL:
			return (matroxfb_DAC_in(minfo, M1064_XVIDPLLM) != m ||
				matroxfb_DAC_in(minfo, M1064_XVIDPLLN) != n ||
				matroxfb_DAC_in(minfo, M1064_XVIDPLLP) != p);
	}
	return 1;
}

static inline int g450_isplllocked(const struct matrox_fb_info *minfo,
				   unsigned int regidx)
{
	unsigned int j;

	for (j = 0; j < 1000; j++) {
		if (matroxfb_DAC_in(minfo, regidx) & 0x40) {
			unsigned int r = 0;
			int i;

			for (i = 0; i < 100; i++) {
				r += matroxfb_DAC_in(minfo, regidx) & 0x40;
			}
			return r >= (90 * 0x40);
		}
		/* udelay(1)... but DAC_in is much slower... */
	}
	return 0;
}

static int g450_testpll(const struct matrox_fb_info *minfo, unsigned int mnp,
			unsigned int pll)
{
	return g450_isplllocked(minfo, g450_setpll(minfo, mnp, pll));
}

static void updatehwstate_clk(struct matrox_hw_state* hw, unsigned int mnp, unsigned int pll) {
	switch (pll) {
		case M_SYSTEM_PLL:
			hw->DACclk[3] = mnp >> 16;
			hw->DACclk[4] = mnp >> 8;
			hw->DACclk[5] = mnp;
			break;
	}
}

void matroxfb_g450_setpll_cond(struct matrox_fb_info *minfo, unsigned int mnp,
			       unsigned int pll)
{
	if (g450_cmppll(minfo, mnp, pll)) {
		g450_setpll(minfo, mnp, pll);
	}
}

static inline unsigned int g450_findworkingpll(struct matrox_fb_info *minfo,
					       unsigned int pll,
					       unsigned int *mnparray,
					       unsigned int mnpcount)
{
	unsigned int found = 0;
	unsigned int idx;
	unsigned int mnpfound = mnparray[0];
		
	for (idx = 0; idx < mnpcount; idx++) {
		unsigned int sarray[3];
		unsigned int *sptr;
		{
			unsigned int mnp;
		
			sptr = sarray;
			mnp = mnparray[idx];
			if (mnp & 0x38) {
				*sptr++ = mnp - 8;
			}
			if ((mnp & 0x38) != 0x38) {
				*sptr++ = mnp + 8;
			}
			*sptr = mnp;
		}
		while (sptr >= sarray) {
			unsigned int mnp = *sptr--;
		
			if (g450_testpll(minfo, mnp - 0x0300, pll) &&
			    g450_testpll(minfo, mnp + 0x0300, pll) &&
			    g450_testpll(minfo, mnp - 0x0200, pll) &&
			    g450_testpll(minfo, mnp + 0x0200, pll) &&
			    g450_testpll(minfo, mnp - 0x0100, pll) &&
			    g450_testpll(minfo, mnp + 0x0100, pll)) {
				if (g450_testpll(minfo, mnp, pll)) {
					return mnp;
				}
			} else if (!found && g450_testpll(minfo, mnp, pll)) {
				mnpfound = mnp;
				found = 1;
			}
		}
	}
	g450_setpll(minfo, mnpfound, pll);
	return mnpfound;
}

static void g450_addcache(struct matrox_pll_cache* ci, unsigned int mnp_key, unsigned int mnp_value) {
	if (++ci->valid > ARRAY_SIZE(ci->data)) {
		ci->valid = ARRAY_SIZE(ci->data);
	}
	memmove(ci->data + 1, ci->data, (ci->valid - 1) * sizeof(*ci->data));
	ci->data[0].mnp_key = mnp_key & G450_MNP_FREQBITS;
	ci->data[0].mnp_value = mnp_value;
}

static int g450_checkcache(struct matrox_fb_info *minfo,
			   struct matrox_pll_cache *ci, unsigned int mnp_key)
{
	unsigned int i;
	
	mnp_key &= G450_MNP_FREQBITS;
	for (i = 0; i < ci->valid; i++) {
		if (ci->data[i].mnp_key == mnp_key) {
			unsigned int mnp;
			
			mnp = ci->data[i].mnp_value;
			if (i) {
				memmove(ci->data + 1, ci->data, i * sizeof(*ci->data));
				ci->data[0].mnp_key = mnp_key;
				ci->data[0].mnp_value = mnp;
			}
			return mnp;
		}
	}
	return NO_MORE_MNP;
}

static int __g450_setclk(struct matrox_fb_info *minfo, unsigned int fout,
			 unsigned int pll, unsigned int *mnparray,
			 unsigned int *deltaarray)
{
	unsigned int mnpcount;
	unsigned int pixel_vco;
	const struct matrox_pll_limits* pi;
	struct matrox_pll_cache* ci;

	pixel_vco = 0;
	switch (pll) {
		case M_PIXEL_PLL_A:
		case M_PIXEL_PLL_B:
		case M_PIXEL_PLL_C:
			{
				u_int8_t tmp, xpwrctrl;
				unsigned long flags;
				
				matroxfb_DAC_lock_irqsave(flags);

				xpwrctrl = matroxfb_DAC_in(minfo, M1064_XPWRCTRL);
				matroxfb_DAC_out(minfo, M1064_XPWRCTRL, xpwrctrl & ~M1064_XPWRCTRL_PANELPDN);
				mga_outb(M_SEQ_INDEX, M_SEQ1);
				mga_outb(M_SEQ_DATA, mga_inb(M_SEQ_DATA) | M_SEQ1_SCROFF);
				tmp = matroxfb_DAC_in(minfo, M1064_XPIXCLKCTRL);
				tmp |= M1064_XPIXCLKCTRL_DIS;
				if (!(tmp & M1064_XPIXCLKCTRL_PLL_UP)) {
					tmp |= M1064_XPIXCLKCTRL_PLL_UP;
				}
				matroxfb_DAC_out(minfo, M1064_XPIXCLKCTRL, tmp);
				/* DVI PLL preferred for frequencies up to
				   panel link max, standard PLL otherwise */
				if (fout >= minfo->max_pixel_clock_panellink)
					tmp = 0;
				else tmp =
					M1064_XDVICLKCTRL_DVIDATAPATHSEL |
					M1064_XDVICLKCTRL_C1DVICLKSEL |
					M1064_XDVICLKCTRL_C1DVICLKEN |
					M1064_XDVICLKCTRL_DVILOOPCTL |
					M1064_XDVICLKCTRL_P1LOOPBWDTCTL;
                                /* Setting this breaks PC systems so don't do it */
				/* matroxfb_DAC_out(minfo, M1064_XDVICLKCTRL, tmp); */
				matroxfb_DAC_out(minfo, M1064_XPWRCTRL,
						 xpwrctrl);

				matroxfb_DAC_unlock_irqrestore(flags);
			}
			{
				u_int8_t misc;
		
				misc = mga_inb(M_MISC_REG_READ) & ~0x0C;
				switch (pll) {
					case M_PIXEL_PLL_A:
						break;
					case M_PIXEL_PLL_B:
						misc |=  0x04;
						break;
					default:
						misc |=  0x0C;
						break;
				}
				mga_outb(M_MISC_REG, misc);
			}
			pi = &minfo->limits.pixel;
			ci = &minfo->cache.pixel;
			break;
		case M_SYSTEM_PLL:
			{
				u_int32_t opt;

				pci_read_config_dword(minfo->pcidev, PCI_OPTION_REG, &opt);
				if (!(opt & 0x20)) {
					pci_write_config_dword(minfo->pcidev, PCI_OPTION_REG, opt | 0x20);
				}
			}
			pi = &minfo->limits.system;
			ci = &minfo->cache.system;
			break;
		case M_VIDEO_PLL:
			{
				u_int8_t tmp;
				unsigned int mnp;
				unsigned long flags;
				
				matroxfb_DAC_lock_irqsave(flags);
				tmp = matroxfb_DAC_in(minfo, M1064_XPWRCTRL);
				if (!(tmp & 2)) {
					matroxfb_DAC_out(minfo, M1064_XPWRCTRL, tmp | 2);
				}
				
				mnp = matroxfb_DAC_in(minfo, M1064_XPIXPLLCM) << 16;
				mnp |= matroxfb_DAC_in(minfo, M1064_XPIXPLLCN) << 8;
				pixel_vco = g450_mnp2vco(minfo, mnp);
				matroxfb_DAC_unlock_irqrestore(flags);
			}
			pi = &minfo->limits.video;
			ci = &minfo->cache.video;
			break;
		default:
			return -EINVAL;
	}

	mnpcount = 0;
	{
		unsigned int mnp;
		unsigned int xvco;

		for (mnp = g450_firstpll(minfo, pi, &xvco, fout); mnp != NO_MORE_MNP; mnp = g450_nextpll(minfo, pi, &xvco, mnp)) {
			unsigned int idx;
			unsigned int vco;
			unsigned int delta;

			vco = g450_mnp2vco(minfo, mnp);
#if 0			
			if (pll == M_VIDEO_PLL) {
				unsigned int big, small;

				if (vco < pixel_vco) {
					small = vco;
					big = pixel_vco;
				} else {
					small = pixel_vco;
					big = vco;
				}
				while (big > small) {
					big >>= 1;
				}
				if (big == small) {
					continue;
				}
			}
#endif			
			delta = pll_freq_delta(fout, g450_vco2f(mnp, vco));
			for (idx = mnpcount; idx > 0; idx--) {
				/* == is important; due to nextpll algorithm we get
				   sorted equally good frequencies from lower VCO 
				   frequency to higher - with <= lowest wins, while
				   with < highest one wins */
				if (delta <= deltaarray[idx-1]) {
					/* all else being equal except VCO,
					 * choose VCO not near (within 1/16th or so) VCOmin
					 * (freqs near VCOmin aren't as stable)
					 */
					if (delta == deltaarray[idx-1]
					    && vco != g450_mnp2vco(minfo, mnparray[idx-1])
					    && vco < (pi->vcomin * 17 / 16)) {
						break;
					}
					mnparray[idx] = mnparray[idx-1];
					deltaarray[idx] = deltaarray[idx-1];
				} else {
					break;
				}
			}
			mnparray[idx] = mnp;
			deltaarray[idx] = delta;
			mnpcount++;
		}
	}
	/* VideoPLL and PixelPLL matched: do nothing... In all other cases we should get at least one frequency */
	if (!mnpcount) {
		return -EBUSY;
	}
	{
		unsigned long flags;
		unsigned int mnp;
		
		matroxfb_DAC_lock_irqsave(flags);
		mnp = g450_checkcache(minfo, ci, mnparray[0]);
		if (mnp != NO_MORE_MNP) {
			matroxfb_g450_setpll_cond(minfo, mnp, pll);
		} else {
			mnp = g450_findworkingpll(minfo, pll, mnparray, mnpcount);
			g450_addcache(ci, mnparray[0], mnp);
		}
		updatehwstate_clk(&minfo->hw, mnp, pll);
		matroxfb_DAC_unlock_irqrestore(flags);
		return mnp;
	}
}

/* It must be greater than number of possible PLL values.
 * Currently there is 5(p) * 10(m) = 50 possible values. */
#define MNP_TABLE_SIZE  64

int matroxfb_g450_setclk(struct matrox_fb_info *minfo, unsigned int fout,
			 unsigned int pll)
{
	unsigned int* arr;
	
	arr = kmalloc(sizeof(*arr) * MNP_TABLE_SIZE * 2, GFP_KERNEL);
	if (arr) {
		int r;

		r = __g450_setclk(minfo, fout, pll, arr, arr + MNP_TABLE_SIZE);
		kfree(arr);
		return r;
	}
	return -ENOMEM;
}

EXPORT_SYMBOL(matroxfb_g450_setclk);
EXPORT_SYMBOL(g450_mnp2f);
EXPORT_SYMBOL(matroxfb_g450_setpll_cond);

MODULE_AUTHOR("(c) 2001-2002 Petr Vandrovec <vandrove@vc.cvut.cz>");
MODULE_DESCRIPTION("Matrox G450/G550 PLL driver");

MODULE_LICENSE("GPL");
Commit	Line	Data
1da177e4 LT	1	/*
	2	*
	3	* Hardware accelerated Matrox PCI cards - G450/G550 PLL control.
	4	*
	5	* (c) 2001-2002 Petr Vandrovec <vandrove@vc.cvut.cz>
	6	*
	7	* Portions Copyright (c) 2001 Matrox Graphics Inc.
	8	*
	9	* Version: 1.64 2002/06/10
	10	*
	11	* This file is subject to the terms and conditions of the GNU General Public
	12	* License. See the file COPYING in the main directory of this archive for
	13	* more details.
	14	*
	15	*/
	16
	17	#include "g450_pll.h"
	18	#include "matroxfb_DAC1064.h"
	19
	20	static inline unsigned int g450_vco2f(unsigned char p, unsigned int fvco) {
	21	return (p & 0x40) ? fvco : fvco >> ((p & 3) + 1);
	22	}
	23
	24	static inline unsigned int g450_f2vco(unsigned char p, unsigned int fin) {
	25	return (p & 0x40) ? fin : fin << ((p & 3) + 1);
	26	}
	27
316b4d64 JD	28	static unsigned int g450_mnp2vco(const struct matrox_fb_info *minfo,
	29	unsigned int mnp)
	30	{
1da177e4 LT	31	unsigned int m, n;
	32
	33	m = ((mnp >> 16) & 0x0FF) + 1;
	34	n = ((mnp >> 7) & 0x1FE) + 4;
fc2d10dd	35	return (minfo->features.pll.ref_freq * n + (m >> 1)) / m;
1da177e4 LT	36	}
1da177e4 LT	37
316b4d64 JD	38	unsigned int g450_mnp2f(const struct matrox_fb_info *minfo, unsigned int mnp)
	39	{
	40	return g450_vco2f(mnp, g450_mnp2vco(minfo, mnp));
1da177e4 LT	41	}
	42
	43	static inline unsigned int pll_freq_delta(unsigned int f1, unsigned int f2) {
	44	if (f2 < f1) {
	45	f2 = f1 - f2;
	46	} else {
	47	f2 = f2 - f1;
	48	}
	49	return f2;
	50	}
	51
	52	#define NO_MORE_MNP 0x01FFFFFF
	53	#define G450_MNP_FREQBITS (0xFFFFFF43) /* do not mask high byte so we'll catch NO_MORE_MNP */
	54
316b4d64 JD	55	static unsigned int g450_nextpll(const struct matrox_fb_info *minfo,
	56	const struct matrox_pll_limits *pi,
	57	unsigned int *fvco, unsigned int mnp)
	58	{
1da177e4 LT	59	unsigned int m, n, p;
	60	unsigned int tvco = *fvco;
	61
	62	m = (mnp >> 16) & 0xFF;
	63	p = mnp & 0xFF;
	64
	65	do {
	66	if (m == 0 \|\| m == 0xFF) {
	67	if (m == 0) {
	68	if (p & 0x40) {
	69	return NO_MORE_MNP;
	70	}
	71	if (p & 3) {
	72	p--;
	73	} else {
	74	p = 0x40;
	75	}
	76	tvco >>= 1;
	77	if (tvco < pi->vcomin) {
	78	return NO_MORE_MNP;
	79	}
	80	*fvco = tvco;
	81	}
	82
	83	p &= 0x43;
	84	if (tvco < 550000) {
	85	/* p \|= 0x00; */
	86	} else if (tvco < 700000) {
	87	p \|= 0x08;
	88	} else if (tvco < 1000000) {
	89	p \|= 0x10;
	90	} else if (tvco < 1150000) {
	91	p \|= 0x18;
	92	} else {
	93	p \|= 0x20;
	94	}
	95	m = 9;
	96	} else {
	97	m--;
	98	}
fc2d10dd	99	n = ((tvco * (m+1) + minfo->features.pll.ref_freq) / (minfo->features.pll.ref_freq * 2)) - 2;
1da177e4 LT	100	} while (n < 0x03 \|\| n > 0x7A);
	101	return (m << 16) \| (n << 8) \| p;
	102	}
	103
316b4d64 JD	104	static unsigned int g450_firstpll(const struct matrox_fb_info *minfo,
	105	const struct matrox_pll_limits *pi,
	106	unsigned int *vco, unsigned int fout)
	107	{
1da177e4 LT	108	unsigned int p;
	109	unsigned int vcomax;
	110
	111	vcomax = pi->vcomax;
	112	if (fout > (vcomax / 2)) {
	113	if (fout > vcomax) {
	114	*vco = vcomax;
	115	} else {
	116	*vco = fout;
	117	}
	118	p = 0x40;
	119	} else {
	120	unsigned int tvco;
	121
	122	p = 3;
	123	tvco = g450_f2vco(p, fout);
	124	while (p && (tvco > vcomax)) {
	125	p--;
	126	tvco >>= 1;
	127	}
	128	if (tvco < pi->vcomin) {
	129	tvco = pi->vcomin;
	130	}
	131	*vco = tvco;
	132	}
316b4d64	133	return g450_nextpll(minfo, pi, vco, 0xFF0000 \| p);
1da177e4 LT	134	}
1da177e4 LT	135
316b4d64 JD	136	static inline unsigned int g450_setpll(const struct matrox_fb_info *minfo,
	137	unsigned int mnp, unsigned int pll)
	138	{
1da177e4 LT	139	switch (pll) {
1da177e4 LT	140	case M_PIXEL_PLL_A:
316b4d64 JD	141	matroxfb_DAC_out(minfo, M1064_XPIXPLLAM, mnp >> 16);
	142	matroxfb_DAC_out(minfo, M1064_XPIXPLLAN, mnp >> 8);
	143	matroxfb_DAC_out(minfo, M1064_XPIXPLLAP, mnp);
1da177e4 LT	144	return M1064_XPIXPLLSTAT;
	145
	146	case M_PIXEL_PLL_B:
316b4d64 JD	147	matroxfb_DAC_out(minfo, M1064_XPIXPLLBM, mnp >> 16);
	148	matroxfb_DAC_out(minfo, M1064_XPIXPLLBN, mnp >> 8);
	149	matroxfb_DAC_out(minfo, M1064_XPIXPLLBP, mnp);
1da177e4 LT	150	return M1064_XPIXPLLSTAT;
	151
	152	case M_PIXEL_PLL_C:
316b4d64 JD	153	matroxfb_DAC_out(minfo, M1064_XPIXPLLCM, mnp >> 16);
	154	matroxfb_DAC_out(minfo, M1064_XPIXPLLCN, mnp >> 8);
	155	matroxfb_DAC_out(minfo, M1064_XPIXPLLCP, mnp);
1da177e4 LT	156	return M1064_XPIXPLLSTAT;
	157
	158	case M_SYSTEM_PLL:
316b4d64 JD	159	matroxfb_DAC_out(minfo, DAC1064_XSYSPLLM, mnp >> 16);
	160	matroxfb_DAC_out(minfo, DAC1064_XSYSPLLN, mnp >> 8);
	161	matroxfb_DAC_out(minfo, DAC1064_XSYSPLLP, mnp);
1da177e4 LT	162	return DAC1064_XSYSPLLSTAT;
	163
	164	case M_VIDEO_PLL:
316b4d64 JD	165	matroxfb_DAC_out(minfo, M1064_XVIDPLLM, mnp >> 16);
	166	matroxfb_DAC_out(minfo, M1064_XVIDPLLN, mnp >> 8);
	167	matroxfb_DAC_out(minfo, M1064_XVIDPLLP, mnp);
1da177e4 LT	168	return M1064_XVIDPLLSTAT;
	169	}
	170	return 0;
	171	}
	172
316b4d64 JD	173	static inline unsigned int g450_cmppll(const struct matrox_fb_info *minfo,
	174	unsigned int mnp, unsigned int pll)
	175	{
1da177e4 LT	176	unsigned char m = mnp >> 16;
	177	unsigned char n = mnp >> 8;
	178	unsigned char p = mnp;
	179
	180	switch (pll) {
	181	case M_PIXEL_PLL_A:
316b4d64 JD	182	return (matroxfb_DAC_in(minfo, M1064_XPIXPLLAM) != m \|\|
	183	matroxfb_DAC_in(minfo, M1064_XPIXPLLAN) != n \|\|
	184	matroxfb_DAC_in(minfo, M1064_XPIXPLLAP) != p);
1da177e4 LT	185
1da177e4 LT	186	case M_PIXEL_PLL_B:
316b4d64 JD	187	return (matroxfb_DAC_in(minfo, M1064_XPIXPLLBM) != m \|\|
	188	matroxfb_DAC_in(minfo, M1064_XPIXPLLBN) != n \|\|
	189	matroxfb_DAC_in(minfo, M1064_XPIXPLLBP) != p);
1da177e4 LT	190
1da177e4 LT	191	case M_PIXEL_PLL_C:
316b4d64 JD	192	return (matroxfb_DAC_in(minfo, M1064_XPIXPLLCM) != m \|\|
	193	matroxfb_DAC_in(minfo, M1064_XPIXPLLCN) != n \|\|
	194	matroxfb_DAC_in(minfo, M1064_XPIXPLLCP) != p);
1da177e4 LT	195
1da177e4 LT	196	case M_SYSTEM_PLL:
316b4d64 JD	197	return (matroxfb_DAC_in(minfo, DAC1064_XSYSPLLM) != m \|\|
	198	matroxfb_DAC_in(minfo, DAC1064_XSYSPLLN) != n \|\|
	199	matroxfb_DAC_in(minfo, DAC1064_XSYSPLLP) != p);
1da177e4 LT	200
1da177e4 LT	201	case M_VIDEO_PLL:
316b4d64 JD	202	return (matroxfb_DAC_in(minfo, M1064_XVIDPLLM) != m \|\|
	203	matroxfb_DAC_in(minfo, M1064_XVIDPLLN) != n \|\|
	204	matroxfb_DAC_in(minfo, M1064_XVIDPLLP) != p);
1da177e4 LT	205	}
	206	return 1;
	207	}
	208
316b4d64 JD	209	static inline int g450_isplllocked(const struct matrox_fb_info *minfo,
	210	unsigned int regidx)
	211	{
1da177e4 LT	212	unsigned int j;
	213
	214	for (j = 0; j < 1000; j++) {
316b4d64	215	if (matroxfb_DAC_in(minfo, regidx) & 0x40) {
1da177e4 LT	216	unsigned int r = 0;
	217	int i;
	218
	219	for (i = 0; i < 100; i++) {
316b4d64	220	r += matroxfb_DAC_in(minfo, regidx) & 0x40;
1da177e4 LT	221	}
	222	return r >= (90 * 0x40);
	223	}
	224	/* udelay(1)... but DAC_in is much slower... */
	225	}
	226	return 0;
	227	}
	228
316b4d64 JD	229	static int g450_testpll(const struct matrox_fb_info *minfo, unsigned int mnp,
	230	unsigned int pll)
	231	{
	232	return g450_isplllocked(minfo, g450_setpll(minfo, mnp, pll));
1da177e4 LT	233	}
	234
	235	static void updatehwstate_clk(struct matrox_hw_state* hw, unsigned int mnp, unsigned int pll) {
	236	switch (pll) {
	237	case M_SYSTEM_PLL:
	238	hw->DACclk[3] = mnp >> 16;
	239	hw->DACclk[4] = mnp >> 8;
	240	hw->DACclk[5] = mnp;
	241	break;
	242	}
	243	}
	244
316b4d64 JD	245	void matroxfb_g450_setpll_cond(struct matrox_fb_info *minfo, unsigned int mnp,
	246	unsigned int pll)
	247	{
	248	if (g450_cmppll(minfo, mnp, pll)) {
	249	g450_setpll(minfo, mnp, pll);
1da177e4 LT	250	}
	251	}
	252
316b4d64 JD	253	static inline unsigned int g450_findworkingpll(struct matrox_fb_info *minfo,
	254	unsigned int pll,
	255	unsigned int *mnparray,
	256	unsigned int mnpcount)
	257	{
1da177e4 LT	258	unsigned int found = 0;
	259	unsigned int idx;
	260	unsigned int mnpfound = mnparray[0];
	261
	262	for (idx = 0; idx < mnpcount; idx++) {
	263	unsigned int sarray[3];
	264	unsigned int *sptr;
	265	{
	266	unsigned int mnp;
	267
	268	sptr = sarray;
	269	mnp = mnparray[idx];
	270	if (mnp & 0x38) {
	271	*sptr++ = mnp - 8;
	272	}
	273	if ((mnp & 0x38) != 0x38) {
	274	*sptr++ = mnp + 8;
	275	}
	276	*sptr = mnp;
	277	}
	278	while (sptr >= sarray) {
	279	unsigned int mnp = *sptr--;
	280
316b4d64 JD	281	if (g450_testpll(minfo, mnp - 0x0300, pll) &&
	282	g450_testpll(minfo, mnp + 0x0300, pll) &&
	283	g450_testpll(minfo, mnp - 0x0200, pll) &&
	284	g450_testpll(minfo, mnp + 0x0200, pll) &&
	285	g450_testpll(minfo, mnp - 0x0100, pll) &&
	286	g450_testpll(minfo, mnp + 0x0100, pll)) {
	287	if (g450_testpll(minfo, mnp, pll)) {
1da177e4 LT	288	return mnp;
1da177e4 LT	289	}
316b4d64	290	} else if (!found && g450_testpll(minfo, mnp, pll)) {
1da177e4 LT	291	mnpfound = mnp;
	292	found = 1;
	293	}
	294	}
	295	}
316b4d64	296	g450_setpll(minfo, mnpfound, pll);
1da177e4 LT	297	return mnpfound;
	298	}
	299
	300	static void g450_addcache(struct matrox_pll_cache* ci, unsigned int mnp_key, unsigned int mnp_value) {
	301	if (++ci->valid > ARRAY_SIZE(ci->data)) {
	302	ci->valid = ARRAY_SIZE(ci->data);
	303	}
	304	memmove(ci->data + 1, ci->data, (ci->valid - 1) * sizeof(*ci->data));
	305	ci->data[0].mnp_key = mnp_key & G450_MNP_FREQBITS;
	306	ci->data[0].mnp_value = mnp_value;
	307	}
	308
316b4d64 JD	309	static int g450_checkcache(struct matrox_fb_info *minfo,
	310	struct matrox_pll_cache *ci, unsigned int mnp_key)
	311	{
1da177e4 LT	312	unsigned int i;
	313
	314	mnp_key &= G450_MNP_FREQBITS;
	315	for (i = 0; i < ci->valid; i++) {
	316	if (ci->data[i].mnp_key == mnp_key) {
	317	unsigned int mnp;
	318
	319	mnp = ci->data[i].mnp_value;
	320	if (i) {
	321	memmove(ci->data + 1, ci->data, i * sizeof(*ci->data));
	322	ci->data[0].mnp_key = mnp_key;
	323	ci->data[0].mnp_value = mnp;
	324	}
	325	return mnp;
	326	}
	327	}
	328	return NO_MORE_MNP;
	329	}
	330
316b4d64 JD	331	static int __g450_setclk(struct matrox_fb_info *minfo, unsigned int fout,
	332	unsigned int pll, unsigned int *mnparray,
	333	unsigned int *deltaarray)
	334	{
1da177e4 LT	335	unsigned int mnpcount;
	336	unsigned int pixel_vco;
	337	const struct matrox_pll_limits* pi;
	338	struct matrox_pll_cache* ci;
	339
	340	pixel_vco = 0;
	341	switch (pll) {
	342	case M_PIXEL_PLL_A:
	343	case M_PIXEL_PLL_B:
	344	case M_PIXEL_PLL_C:
	345	{
6d39bedc	346	u_int8_t tmp, xpwrctrl;
1da177e4 LT	347	unsigned long flags;
	348
	349	matroxfb_DAC_lock_irqsave(flags);
6d39bedc	350
316b4d64 JD	351	xpwrctrl = matroxfb_DAC_in(minfo, M1064_XPWRCTRL);
316b4d64 JD	352	matroxfb_DAC_out(minfo, M1064_XPWRCTRL, xpwrctrl & ~M1064_XPWRCTRL_PANELPDN);
6d39bedc PC	353	mga_outb(M_SEQ_INDEX, M_SEQ1);
6d39bedc PC	354	mga_outb(M_SEQ_DATA, mga_inb(M_SEQ_DATA) \| M_SEQ1_SCROFF);
316b4d64	355	tmp = matroxfb_DAC_in(minfo, M1064_XPIXCLKCTRL);
6d39bedc	356	tmp \|= M1064_XPIXCLKCTRL_DIS;
1da177e4	357	if (!(tmp & M1064_XPIXCLKCTRL_PLL_UP)) {
6d39bedc	358	tmp \|= M1064_XPIXCLKCTRL_PLL_UP;
1da177e4	359	}
316b4d64	360	matroxfb_DAC_out(minfo, M1064_XPIXCLKCTRL, tmp);
e798bd95 PC	361	/* DVI PLL preferred for frequencies up to
e798bd95 PC	362	panel link max, standard PLL otherwise */
fc2d10dd	363	if (fout >= minfo->max_pixel_clock_panellink)
e798bd95 PC	364	tmp = 0;
	365	else tmp =
	366	M1064_XDVICLKCTRL_DVIDATAPATHSEL \|
	367	M1064_XDVICLKCTRL_C1DVICLKSEL \|
	368	M1064_XDVICLKCTRL_C1DVICLKEN \|
	369	M1064_XDVICLKCTRL_DVILOOPCTL \|
	370	M1064_XDVICLKCTRL_P1LOOPBWDTCTL;
8c651311 AC	371	/* Setting this breaks PC systems so don't do it */
8c651311 AC	372	/* matroxfb_DAC_out(minfo, M1064_XDVICLKCTRL, tmp); */
316b4d64	373	matroxfb_DAC_out(minfo, M1064_XPWRCTRL,
e798bd95	374	xpwrctrl);
6d39bedc	375
1da177e4 LT	376	matroxfb_DAC_unlock_irqrestore(flags);
	377	}
	378	{
	379	u_int8_t misc;
	380
	381	misc = mga_inb(M_MISC_REG_READ) & ~0x0C;
	382	switch (pll) {
	383	case M_PIXEL_PLL_A:
	384	break;
	385	case M_PIXEL_PLL_B:
	386	misc \|= 0x04;
	387	break;
	388	default:
	389	misc \|= 0x0C;
	390	break;
	391	}
	392	mga_outb(M_MISC_REG, misc);
	393	}
fc2d10dd JD	394	pi = &minfo->limits.pixel;
fc2d10dd JD	395	ci = &minfo->cache.pixel;
1da177e4 LT	396	break;
	397	case M_SYSTEM_PLL:
	398	{
	399	u_int32_t opt;
	400
fc2d10dd	401	pci_read_config_dword(minfo->pcidev, PCI_OPTION_REG, &opt);
1da177e4	402	if (!(opt & 0x20)) {
fc2d10dd	403	pci_write_config_dword(minfo->pcidev, PCI_OPTION_REG, opt \| 0x20);
1da177e4 LT	404	}
1da177e4 LT	405	}
fc2d10dd JD	406	pi = &minfo->limits.system;
fc2d10dd JD	407	ci = &minfo->cache.system;
1da177e4 LT	408	break;
	409	case M_VIDEO_PLL:
	410	{
	411	u_int8_t tmp;
	412	unsigned int mnp;
	413	unsigned long flags;
	414
	415	matroxfb_DAC_lock_irqsave(flags);
316b4d64	416	tmp = matroxfb_DAC_in(minfo, M1064_XPWRCTRL);
1da177e4	417	if (!(tmp & 2)) {
316b4d64	418	matroxfb_DAC_out(minfo, M1064_XPWRCTRL, tmp \| 2);
1da177e4 LT	419	}
1da177e4 LT	420
316b4d64 JD	421	mnp = matroxfb_DAC_in(minfo, M1064_XPIXPLLCM) << 16;
	422	mnp \|= matroxfb_DAC_in(minfo, M1064_XPIXPLLCN) << 8;
	423	pixel_vco = g450_mnp2vco(minfo, mnp);
1da177e4 LT	424	matroxfb_DAC_unlock_irqrestore(flags);
1da177e4 LT	425	}
fc2d10dd JD	426	pi = &minfo->limits.video;
fc2d10dd JD	427	ci = &minfo->cache.video;
1da177e4 LT	428	break;
	429	default:
	430	return -EINVAL;
	431	}
	432
	433	mnpcount = 0;
	434	{
	435	unsigned int mnp;
	436	unsigned int xvco;
	437
316b4d64	438	for (mnp = g450_firstpll(minfo, pi, &xvco, fout); mnp != NO_MORE_MNP; mnp = g450_nextpll(minfo, pi, &xvco, mnp)) {
1da177e4 LT	439	unsigned int idx;
	440	unsigned int vco;
	441	unsigned int delta;
	442
316b4d64	443	vco = g450_mnp2vco(minfo, mnp);
1da177e4 LT	444	#if 0
	445	if (pll == M_VIDEO_PLL) {
	446	unsigned int big, small;
	447
	448	if (vco < pixel_vco) {
	449	small = vco;
	450	big = pixel_vco;
	451	} else {
	452	small = pixel_vco;
	453	big = vco;
	454	}
	455	while (big > small) {
	456	big >>= 1;
	457	}
	458	if (big == small) {
	459	continue;
	460	}
	461	}
	462	#endif
	463	delta = pll_freq_delta(fout, g450_vco2f(mnp, vco));
	464	for (idx = mnpcount; idx > 0; idx--) {
	465	/* == is important; due to nextpll algorithm we get
	466	sorted equally good frequencies from lower VCO
	467	frequency to higher - with <= lowest wins, while
	468	with < highest one wins */
	469	if (delta <= deltaarray[idx-1]) {
6d39bedc PC	470	/* all else being equal except VCO,
	471	* choose VCO not near (within 1/16th or so) VCOmin
	472	* (freqs near VCOmin aren't as stable)
	473	*/
	474	if (delta == deltaarray[idx-1]
316b4d64	475	&& vco != g450_mnp2vco(minfo, mnparray[idx-1])
6d39bedc PC	476	&& vco < (pi->vcomin * 17 / 16)) {
	477	break;
	478	}
1da177e4 LT	479	mnparray[idx] = mnparray[idx-1];
	480	deltaarray[idx] = deltaarray[idx-1];
	481	} else {
	482	break;
	483	}
	484	}
	485	mnparray[idx] = mnp;
	486	deltaarray[idx] = delta;
	487	mnpcount++;
	488	}
	489	}
	490	/* VideoPLL and PixelPLL matched: do nothing... In all other cases we should get at least one frequency */
	491	if (!mnpcount) {
	492	return -EBUSY;
	493	}
	494	{
	495	unsigned long flags;
	496	unsigned int mnp;
	497
	498	matroxfb_DAC_lock_irqsave(flags);
316b4d64	499	mnp = g450_checkcache(minfo, ci, mnparray[0]);
1da177e4	500	if (mnp != NO_MORE_MNP) {
316b4d64	501	matroxfb_g450_setpll_cond(minfo, mnp, pll);
1da177e4	502	} else {
316b4d64	503	mnp = g450_findworkingpll(minfo, pll, mnparray, mnpcount);
1da177e4 LT	504	g450_addcache(ci, mnparray[0], mnp);
1da177e4 LT	505	}
fc2d10dd	506	updatehwstate_clk(&minfo->hw, mnp, pll);
1da177e4 LT	507	matroxfb_DAC_unlock_irqrestore(flags);
	508	return mnp;
	509	}
	510	}
	511
	512	/* It must be greater than number of possible PLL values.
	513	* Currently there is 5(p) * 10(m) = 50 possible values. */
	514	#define MNP_TABLE_SIZE 64
	515
316b4d64 JD	516	int matroxfb_g450_setclk(struct matrox_fb_info *minfo, unsigned int fout,
	517	unsigned int pll)
	518	{
1da177e4 LT	519	unsigned int* arr;
	520
	521	arr = kmalloc(sizeof(arr) MNP_TABLE_SIZE * 2, GFP_KERNEL);
	522	if (arr) {
	523	int r;
	524
316b4d64	525	r = __g450_setclk(minfo, fout, pll, arr, arr + MNP_TABLE_SIZE);
1da177e4 LT	526	kfree(arr);
	527	return r;
	528	}
	529	return -ENOMEM;
	530	}
	531
	532	EXPORT_SYMBOL(matroxfb_g450_setclk);
	533	EXPORT_SYMBOL(g450_mnp2f);
	534	EXPORT_SYMBOL(matroxfb_g450_setpll_cond);
	535
	536	MODULE_AUTHOR("(c) 2001-2002 Petr Vandrovec <vandrove@vc.cvut.cz>");
	537	MODULE_DESCRIPTION("Matrox G450/G550 PLL driver");
	538
	539	MODULE_LICENSE("GPL");