/*
   * (C) Copyright 1997-2002 ELTEC Elektronik AG
   * Frank Gottschling <fgottschling@eltec.de>
   *
   * SPDX-License-Identifier:	GPL-2.0+
   */
  
  /*
   * smiLynxEM.c
   *
   * Silicon Motion graphic interface for sm810/sm710/sm712 accelerator
   *
   * modification history
   * --------------------
   * 04-18-2002 Rewritten for U-Boot <fgottschling@eltec.de>.
   *
   * 18-03-2004 - Unify videomodes handling with the ct69000
   *            - The video output can be set via the variable "videoout"
   *              in the environment.
   *              videoout=1 output on LCD
   *              videoout=2 output on CRT (default value)
   *	                <p.aubert@staubli.com>
   */
  
  #include <common.h>
  
  #include <pci.h>
  #include <video_fb.h>
  #include "videomodes.h"
  /*
   * Export Graphic Device
   */
  GraphicDevice smi;
  
  /*
   * SMI 710/712 have 4MB internal RAM; SMI 810 2MB internal + 2MB external
   */
  #define VIDEO_MEM_SIZE	0x400000
  
  
  /*
   * ISA mapped regs
   */
  #define SMI_INDX_C4		(pGD->isaBase + 0x03c4)	   /* index reg */
  #define SMI_DATA_C5		(pGD->isaBase + 0x03c5)	   /* data reg */
  #define SMI_INDX_D4		(pGD->isaBase + 0x03d4)	   /* index reg */
  #define SMI_DATA_D5		(pGD->isaBase + 0x03d5)	   /* data reg */
  #define SMI_ISR1		(pGD->isaBase + 0x03ca)
  #define SMI_INDX_CE		(pGD->isaBase + 0x03ce)	   /* index reg */
  #define SMI_DATA_CF		(pGD->isaBase + 0x03cf)	   /* data reg */
  #define SMI_LOCK_REG		(pGD->isaBase + 0x03c3)	   /* unlock/lock ext crt reg */
  #define SMI_MISC_REG		(pGD->isaBase + 0x03c2)	   /* misc reg */
  #define SMI_LUT_MASK		(pGD->isaBase + 0x03c6)	   /* lut mask reg */
  #define SMI_LUT_START		(pGD->isaBase + 0x03c8)	   /* lut start index */
  #define SMI_LUT_RGB		(pGD->isaBase + 0x03c9)	   /* lut colors auto incr.*/
  #define SMI_INDX_ATTR		(pGD->isaBase + 0x03c0)	   /* attributes index reg */
  
  /*
   * Video processor control
   */
  typedef struct {
  	unsigned int   control;
  	unsigned int   colorKey;
  	unsigned int   colorKeyMask;
  	unsigned int   start;
  	unsigned short offset;
  	unsigned short width;
  	unsigned int   fifoPrio;
  	unsigned int   fifoERL;
  	unsigned int   YUVtoRGB;
  } SmiVideoProc;
  
  /*
   * Video window control
   */
  typedef struct {
  	unsigned short top;
  	unsigned short left;
  	unsigned short bottom;
  	unsigned short right;
  	unsigned int   srcStart;
  	unsigned short width;
  	unsigned short offset;
  	unsigned char  hStretch;
  	unsigned char  vStretch;
  } SmiVideoWin;
  
  /*
   * Capture port control
   */
  typedef struct {
  	unsigned int   control;
  	unsigned short topClip;
  	unsigned short leftClip;
  	unsigned short srcHeight;
  	unsigned short srcWidth;
  	unsigned int   srcBufStart1;
  	unsigned int   srcBufStart2;
  	unsigned short srcOffset;
  	unsigned short fifoControl;
  } SmiCapturePort;
  
  
  /*
   * Register values for common video modes
   */
  static char SMI_SCR[] = {
  	/* all modes */
  	0x10, 0xff, 0x11, 0xff, 0x12, 0xff, 0x13, 0xff, 0x15, 0x90,
  	0x17, 0x20, 0x18, 0xb1, 0x19, 0x00,
  };
  static char SMI_EXT_CRT[] = {
  	0x31, 0x00, 0x32, 0x00, 0x33, 0x00, 0x34, 0x00, 0x35, 0x00,
  	0x36, 0x00, 0x3b, 0x00, 0x3c, 0x00, 0x3d, 0x00, 0x3e, 0x00, 0x3f, 0x00,
  };
  static char SMI_ATTR [] = {
  	0x00, 0x00, 0x01, 0x01, 0x02, 0x02, 0x03, 0x03, 0x04, 0x04, 0x05, 0x05,
  	0x06, 0x06, 0x07, 0x07, 0x08, 0x08, 0x09, 0x09, 0x0a, 0x0a, 0x0b, 0x0b,
  	0x0c, 0x0c, 0x0d, 0x0d, 0x0e, 0x0e, 0x0f, 0x0f, 0x10, 0x41, 0x11, 0x00,
  	0x12, 0x0f, 0x13, 0x00, 0x14, 0x00,
  };
  static char SMI_GCR[18] = {
  	0x00, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x04, 0x00, 0x05, 0x40,
  	0x06, 0x05, 0x07, 0x0f, 0x08, 0xff,
  };
  static char SMI_SEQR[] = {
  	0x00, 0x00, 0x01, 0x01, 0x02, 0x0f, 0x03, 0x03, 0x04, 0x0e, 0x00, 0x03,
  };
  static char SMI_PCR [] = {
  	0x20, 0x04, 0x21, 0x30, 0x22, 0x00, 0x23, 0x00, 0x24, 0x00,
  };
  static char SMI_MCR[] = {
  	0x60, 0x01, 0x61, 0x00,
  };
  
  static char SMI_HCR[] = {
  	0x80, 0xff, 0x81, 0x07, 0x82, 0x00, 0x83, 0xff, 0x84, 0xff, 0x88, 0x00,
  	0x89, 0x02, 0x8a, 0x80, 0x8b, 0x01, 0x8c, 0xff, 0x8d, 0x00,
  };
  
  
  /*******************************************************************************
   *
   * Write SMI ISA register
   */
  static void smiWrite (unsigned short index, char reg, char val)
  {
  	register GraphicDevice *pGD = (GraphicDevice *)&smi;
  
  	out8 ((pGD->isaBase + index), reg);
  	out8 ((pGD->isaBase + index + 1), val);
  }
  
  /*******************************************************************************
   *
   * Write a table of SMI ISA register
   */
  static void smiLoadRegs (
  	unsigned int iReg,
  	unsigned int dReg,
  	char	     *regTab,
  	unsigned int tabSize
  	)
  {
  	register GraphicDevice *pGD  = (GraphicDevice *)&smi;
  	register int i;
  
  	for (i=0; i<tabSize; i+=2) {
  		if (iReg == SMI_INDX_ATTR) {
  			/* Reset the Flip Flop */
  			in8 (SMI_ISR1);
  			out8 (iReg, regTab[i]);
  			out8 (iReg, regTab[i+1]);
  		} else {
  			out8 (iReg, regTab[i]);
  			out8 (dReg, regTab[i+1]);
  		}
  	}
  }
  
  /*******************************************************************************
   *
   * Init capture port registers
   */
  static void smiInitCapturePort (void)
  {
  	SmiCapturePort smiCP = { 0x01400600, 0x30, 0x40, 480, 640, 0, 0, 2560, 6 };
  	register GraphicDevice *pGD  = (GraphicDevice *)&smi;
  	register SmiCapturePort *pCP = (SmiCapturePort *)&smiCP;
  
  	out32r ((pGD->cprBase + 0x0004), ((pCP->topClip<<16)   | pCP->leftClip));
  	out32r ((pGD->cprBase + 0x0008), ((pCP->srcHeight<<16) | pCP->srcWidth));
  	out32r ((pGD->cprBase + 0x000c), pCP->srcBufStart1/8);
  	out32r ((pGD->cprBase + 0x0010), pCP->srcBufStart2/8);
  	out32r ((pGD->cprBase + 0x0014), pCP->srcOffset/8);
  	out32r ((pGD->cprBase + 0x0018), pCP->fifoControl);
  	out32r ((pGD->cprBase + 0x0000), pCP->control);
  }
  
  
  /*******************************************************************************
   *
   * Init video processor registers
   */
  static void smiInitVideoProcessor (void)
  {
  	SmiVideoProc smiVP = { 0x100000, 0, 0, 0, 0, 1600, 0x1200543, 4, 0xededed };
  	SmiVideoWin  smiVW = { 0, 0, 599, 799, 0, 1600, 0, 0, 0 };
  	register GraphicDevice *pGD = (GraphicDevice *)&smi;
  	register SmiVideoProc  *pVP = (SmiVideoProc *)&smiVP;
  	register SmiVideoWin *pVWin = (SmiVideoWin *)&smiVW;
  
  	pVP->width    = pGD->plnSizeX * pGD->gdfBytesPP;
  	pVP->control |= pGD->gdfIndex << 16;
  	pVWin->bottom = pGD->winSizeY - 1;
  	pVWin->right  = pGD->winSizeX - 1;
  	pVWin->width  = pVP->width;
  
  	/* color key */
  	out32r ((pGD->vprBase + 0x0004), pVP->colorKey);
  
  	/* color key mask */
  	out32r ((pGD->vprBase + 0x0008), pVP->colorKeyMask);
  
  	/* data src start adrs */
  	out32r ((pGD->vprBase + 0x000c), pVP->start / 8);
  
  	/* data width and offset */
  	out32r ((pGD->vprBase + 0x0010),
  		((pVP->offset	/ 8 * pGD->gdfBytesPP) << 16) |
  		(pGD->plnSizeX / 8 * pGD->gdfBytesPP));
  
  	/* video window 1 */
  	out32r ((pGD->vprBase + 0x0014),
  		((pVWin->top << 16) | pVWin->left));
  
  	out32r ((pGD->vprBase + 0x0018),
  		((pVWin->bottom << 16) | pVWin->right));
  
  	out32r ((pGD->vprBase + 0x001c), pVWin->srcStart / 8);
  
  	out32r ((pGD->vprBase + 0x0020),
  		(((pVWin->offset / 8) << 16) | (pVWin->width / 8)));
  
  	out32r ((pGD->vprBase + 0x0024),
  		(((pVWin->hStretch) << 8) | pVWin->vStretch));
  
  	/* video window 2 */
  	out32r ((pGD->vprBase + 0x0028),
  		((pVWin->top << 16) | pVWin->left));
  
  	out32r ((pGD->vprBase + 0x002c),
  		((pVWin->bottom << 16) | pVWin->right));
  
  	out32r ((pGD->vprBase + 0x0030),
  		pVWin->srcStart / 8);
  
  	out32r ((pGD->vprBase + 0x0034),
  		(((pVWin->offset / 8) << 16) | (pVWin->width / 8)));
  
  	out32r ((pGD->vprBase + 0x0038),
  		(((pVWin->hStretch) << 8) | pVWin->vStretch));
  
  	/* fifo prio control */
  	out32r ((pGD->vprBase + 0x0054), pVP->fifoPrio);
  
  	/* fifo empty request levell */
  	out32r ((pGD->vprBase + 0x0058), pVP->fifoERL);
  
  	/* conversion constant */
  	out32r ((pGD->vprBase + 0x005c), pVP->YUVtoRGB);
  
  	/* vpr control word */
  	out32r ((pGD->vprBase + 0x0000), pVP->control);
  }
  
  /******************************************************************************
   *
   * Init drawing engine registers
   */
  static void smiInitDrawingEngine (void)
  {
  	GraphicDevice *pGD = (GraphicDevice *)&smi;
  	unsigned int val;
  
  	/* don't start now */
  	out32r ((pGD->dprBase + 0x000c), 0x000f0000);
  
  	/* set rop2 to copypen */
  	val = 0xffff3ff0 & in32r ((pGD->dprBase + 0x000c));
  	out32r ((pGD->dprBase + 0x000c), (val | 0x8000 | 0x0c));
  
  	/* set clip rect */
  	out32r ((pGD->dprBase + 0x002c), 0);
  	out32r ((pGD->dprBase + 0x0030),
  		((pGD->winSizeY<<16) | pGD->winSizeX * pGD->gdfBytesPP ));
  
  	/* src row pitch */
  	val = 0xffff0000 & (in32r ((pGD->dprBase + 0x0010)));
  	out32r ((pGD->dprBase + 0x0010),
  		(val | pGD->plnSizeX * pGD->gdfBytesPP));
  
  	/* dst row pitch */
  	val = 0x0000ffff & (in32r ((pGD->dprBase + 0x0010)));
  	out32r ((pGD->dprBase + 0x0010),
  		(((pGD->plnSizeX * pGD->gdfBytesPP)<<16) | val));
  
  	/* window width src/dst */
  	out32r ((pGD->dprBase + 0x003c),
  		(((pGD->plnSizeX * pGD->gdfBytesPP & 0x0fff)<<16) |
  		 (pGD->plnSizeX * pGD->gdfBytesPP & 0x0fff)));
  	out16r ((pGD->dprBase + 0x001e), 0x0000);
  
  	/* src base adrs */
  	out32r ((pGD->dprBase + 0x0040),
  		(((pGD->frameAdrs/8) & 0x000fffff)));
  
  	/* dst base adrs */
  	out32r ((pGD->dprBase + 0x0044),
  		(((pGD->frameAdrs/8) & 0x000fffff)));
  
  	/* foreground color */
  	out32r ((pGD->dprBase + 0x0014), pGD->fg);
  
  	/* background color */
  	out32r ((pGD->dprBase + 0x0018), pGD->bg);
  
  	/* xcolor */
  	out32r ((pGD->dprBase + 0x0020), 0x00ffffff);
  
  	/* xcolor mask */
  	out32r ((pGD->dprBase + 0x0024), 0x00ffffff);
  
  	/* bit mask */
  	out32r ((pGD->dprBase + 0x0028), 0x00ffffff);
  
  	/* load mono pattern */
  	out32r ((pGD->dprBase + 0x0034), 0);
  	out32r ((pGD->dprBase + 0x0038), 0);
  }
  
  static struct pci_device_id supported[] = {
  	{ PCI_VENDOR_ID_SMI, PCI_DEVICE_ID_SMI_710 },
  	{ PCI_VENDOR_ID_SMI, PCI_DEVICE_ID_SMI_712 },
  	{ PCI_VENDOR_ID_SMI, PCI_DEVICE_ID_SMI_810 },
  	{ }
  };
  
  /*****************************************************************************/
  static void smiLoadMsr (struct ctfb_res_modes *mode)
  {
  	unsigned char h_synch_high, v_synch_high;
  	register GraphicDevice *pGD  = (GraphicDevice *)&smi;
  
  	h_synch_high = (mode->sync & FB_SYNC_HOR_HIGH_ACT) ? 0 : 0x40;	/* horizontal Synch High active */
  	v_synch_high = (mode->sync & FB_SYNC_VERT_HIGH_ACT) ? 0 : 0x80;	/* vertical Synch High active */
  	out8 (SMI_MISC_REG, (h_synch_high | v_synch_high | 0x29));
  	/* upper64K==0x20, CLC2select==0x08, RAMenable==0x02!(todo), CGA==0x01
  	 * Selects the upper 64KB page.Bit5=1
  	 * CLK2 (left reserved in standard VGA) Bit3|2=1|0
  	 * Disables CPU access to frame buffer. Bit1=0
  	 * Sets the I/O address decode for ST01, FCR, and all CR registers
  	 * to the 3Dx I/O address range (CGA emulation). Bit0=1
  	 */
  }
  /*****************************************************************************/
  static void smiLoadCrt (struct ctfb_res_modes *var, int bits_per_pixel)
  {
  	unsigned char cr[0x7a];
  	int i;
  	unsigned int hd, hs, he, ht, hbs, hbe;	/* Horizontal.	*/
  	unsigned int vd, vs, ve, vt, vbs, vbe;	/* vertical */
  	unsigned int bpp, wd, dblscan, interlaced;
  
  	const int LineCompare = 0x3ff;
  	unsigned int TextScanLines = 1;	/* this is in fact a vertical zoom factor   */
  	register GraphicDevice *pGD  = (GraphicDevice *)&smi;
  
  	/* Horizontal */
  	hd = (var->xres) / 8;	/* HDisp.  */
  	hs = (var->xres + var->right_margin) / 8;	/* HsStrt  */
  	he = (var->xres + var->right_margin + var->hsync_len) / 8;	/* HsEnd   */
  	ht = (var->left_margin + var->xres + var->right_margin + var->hsync_len) / 8;	/* HTotal  */
  	/* Blank */
  	hbs = hd;
  	hbe = 0; /* Blank end at 0 */
  
  	/* Vertical */
  	vd = var->yres;		/* VDisplay   */
  	vs = var->yres + var->lower_margin;	/* VSyncStart */
  	ve = var->yres + var->lower_margin + var->vsync_len;	/* VSyncEnd */
  	vt = var->upper_margin + var->yres + var->lower_margin + var->vsync_len;	/* VTotal  */
  	vbs = vd;
  	vbe = 0;
  
  	bpp = bits_per_pixel;
  	dblscan = (var->vmode & FB_VMODE_DOUBLE) ? 1 : 0;
  	interlaced = var->vmode & FB_VMODE_INTERLACED;
  
  
  	if (bpp == 15)
  		bpp = 16;
  	wd = var->xres * bpp / 64;	/* double words per line */
  	if (interlaced) {	/* we divide all vertical timings, exept vd */
  		vs >>= 1;
  		vbs >>= 1;
  		ve >>= 1;
  		vt >>= 1;
  	}
  
  	memset (cr, 0, sizeof (cr));
  	cr[0x00] = ht - 5;
  	cr[0x01] = hd - 1;
  	cr[0x02] = hbs - 1;
  	cr[0x03] = (hbe & 0x1F);
  	cr[0x04] = hs;
  	cr[0x05] = ((hbe & 0x20) << 2) | (he & 0x1f);
  
  	cr[0x06] = (vt - 2) & 0xFF;
  	cr[0x07] = (((vt - 2) & 0x100) >> 8)
  		| (((vd - 1) & 0x100) >> 7)
  		| ((vs & 0x100) >> 6)
  		| (((vbs - 1) & 0x100) >> 5)
  		| ((LineCompare & 0x100) >> 4)
  		| (((vt - 2) & 0x200) >> 4)
  		| (((vd - 1) & 0x200) >> 3)
  		| ((vs & 0x200) >> 2);
  
  	cr[0x30] = ((vt - 2) & 0x400) >> 7
  		| (((vd - 1) & 0x400) >> 8)
  		| (((vbs - 1) & 0x400) >> 9)
  		| ((vs & 0x400) >> 10)
  		| (interlaced) ? 0x80 : 0;
  
  
  	cr[0x08] = 0x00;
  	cr[0x09] = (dblscan << 7)
  		| ((LineCompare & 0x200) >> 3)
  		| (((vbs - 1) & 0x200) >> 4)
  		| (TextScanLines - 1);
  
  	cr[0x10] = vs & 0xff;	/* VSyncPulseStart */
  	cr[0x11] = (ve & 0x0f);
  	cr[0x12] = (vd - 1) & 0xff;	/* LineCount  */
  	cr[0x13] = wd & 0xff;
  	cr[0x14] = 0x40;
  	cr[0x15] = (vbs - 1) & 0xff;
  	cr[0x16] = vbe & 0xff;
  	cr[0x17] = 0xe3;	/* but it does not work */
  	cr[0x18] = 0xff & LineCompare;
  	cr[0x22] = 0x00;	/* todo? */
  
  
  	/* now set the registers */
  	for (i = 0; i <= 0x18; i++) {	/*CR00 .. CR18 */
  		smiWrite (SMI_INDX_D4, i, cr[i]);
  	}
  	i = 0x22;		/*CR22 */
  	smiWrite (SMI_INDX_D4, i, cr[i]);
  	i = 0x30;		/*CR30 */
  	smiWrite (SMI_INDX_D4, i, cr[i]);
  }
  
  /*****************************************************************************/
  #define REF_FREQ	14318180
  #define PMIN		1
  #define PMAX		255
  #define QMIN		1
  #define QMAX		63
  
  static unsigned int FindPQ (unsigned int freq, unsigned int *pp, unsigned int *pq)
  {
  	unsigned int n = QMIN, m = 0;
  	long long int L = 0, P = freq, Q = REF_FREQ, H = P >> 1;
  	long long int D = 0x7ffffffffffffffLL;
  
  	for (n = QMIN; n <= QMAX; n++) {
  		m = PMIN;	/* p/q ~ freq/ref -> p*ref-freq*q ~ 0 */
  		L = P * n - m * Q;
  		while (L > 0 && m < PMAX) {
  			L -= REF_FREQ;	/* difference is greater as 0 subtract fref */
  			m++;	/* and increment m */
  		}
  		/* difference is less or equal than 0 or m > maximum */
  		if (m > PMAX)
  			break;	/* no solution: if we increase n we get the same situation */
  		/* L is <= 0 now */
  		if (-L > H && m > PMIN) {	/* if difference > the half fref */
  			L += REF_FREQ;	/* we take the situation before */
  			m--;	/* because its closer to 0 */
  		}
  		L = (L < 0) ? -L : +L;	/* absolute value */
  		if (D < L)	/* if last difference was better take next n */
  			continue;
  		D = L;
  		*pp = m;
  		*pq = n;	/*  keep improved data */
  		if (D == 0)
  			break;	/* best result we can get */
  	}
  	return (unsigned int) (0xffffffff & D);
  }
  
  /*****************************************************************************/
  static void smiLoadCcr (struct ctfb_res_modes *var, unsigned short device_id)
  {
  	unsigned int p = 0;
  	unsigned int q = 0;
  	long long freq;
  	register GraphicDevice *pGD  = (GraphicDevice *)&smi;
  
  	smiWrite (SMI_INDX_C4, 0x65, 0);
  	smiWrite (SMI_INDX_C4, 0x66, 0);
  	smiWrite (SMI_INDX_C4, 0x68, 0x50);
  	if (device_id == PCI_DEVICE_ID_SMI_810) {
  		smiWrite (SMI_INDX_C4, 0x69, 0x3);
  	} else {
  		smiWrite (SMI_INDX_C4, 0x69, 0x0);
  	}
  
  	/* Memory clock */
  	switch (device_id) {
  	case PCI_DEVICE_ID_SMI_710 :
  		smiWrite (SMI_INDX_C4, 0x6a, 0x75);
  		break;
  	case PCI_DEVICE_ID_SMI_712 :
  		smiWrite (SMI_INDX_C4, 0x6a, 0x80);
  		break;
  	default :
  		smiWrite (SMI_INDX_C4, 0x6a, 0x53);
  		break;
  	}
  	smiWrite (SMI_INDX_C4, 0x6b, 0x15);
  
  	/* VCLK */
  	freq = 1000000000000LL / var -> pixclock;
  
  	FindPQ ((unsigned int)freq, &p, &q);
  
  	smiWrite (SMI_INDX_C4, 0x6c, p);
  	smiWrite (SMI_INDX_C4, 0x6d, q);
  
  }
  
  /*******************************************************************************
   *
   * Init video chip with common Linux graphic modes (lilo)
   */
  void *video_hw_init (void)
  {
  	GraphicDevice *pGD = (GraphicDevice *)&smi;
  	unsigned short device_id;
  	pci_dev_t devbusfn;
  	int videomode;
  	unsigned long t1, hsynch, vsynch;
  	unsigned int pci_mem_base, *vm;
  	char *penv;
  	int tmp, i, bits_per_pixel;
  	struct ctfb_res_modes *res_mode;
  	struct ctfb_res_modes var_mode;
  	unsigned char videoout;
  
  	/* Search for video chip */
  	printf("Video: ");
  
  	if ((devbusfn = pci_find_devices(supported, 0)) < 0)
  	{
  		printf ("Controller not found !
  ");
  		return (NULL);
  	}
  
  	/* PCI setup */
  	pci_write_config_dword (devbusfn, PCI_COMMAND, (PCI_COMMAND_MEMORY | PCI_COMMAND_IO));
  	pci_read_config_word (devbusfn, PCI_DEVICE_ID, &device_id);
  	pci_read_config_dword (devbusfn, PCI_BASE_ADDRESS_0, &pci_mem_base);
  	pci_mem_base = pci_mem_to_phys (devbusfn, pci_mem_base);
  
  	tmp = 0;
  
  	videomode = CONFIG_SYS_DEFAULT_VIDEO_MODE;
  	/* get video mode via environment */
  	if ((penv = getenv ("videomode")) != NULL) {
  		/* deceide if it is a string */
  		if (penv[0] <= '9') {
  			videomode = (int) simple_strtoul (penv, NULL, 16);
  			tmp = 1;
  		}
  	} else {
  		tmp = 1;
  	}
  	if (tmp) {
  		/* parameter are vesa modes */
  		/* search params */
  		for (i = 0; i < VESA_MODES_COUNT; i++) {
  			if (vesa_modes[i].vesanr == videomode)
  				break;
  		}
  		if (i == VESA_MODES_COUNT) {
  			printf ("no VESA Mode found, switching to mode 0x%x ", CONFIG_SYS_DEFAULT_VIDEO_MODE);
  			i = 0;
  		}
  		res_mode =
  			(struct ctfb_res_modes *) &res_mode_init[vesa_modes[i].
  								 resindex];
  		bits_per_pixel = vesa_modes[i].bits_per_pixel;
  	} else {
  
  		res_mode = (struct ctfb_res_modes *) &var_mode;
  		bits_per_pixel = video_get_params (res_mode, penv);
  	}
  
  	/* calculate hsynch and vsynch freq (info only) */
  	t1 = (res_mode->left_margin + res_mode->xres +
  	      res_mode->right_margin + res_mode->hsync_len) / 8;
  	t1 *= 8;
  	t1 *= res_mode->pixclock;
  	t1 /= 1000;
  	hsynch = 1000000000L / t1;
  	t1 *=
  		(res_mode->upper_margin + res_mode->yres +
  		 res_mode->lower_margin + res_mode->vsync_len);
  	t1 /= 1000;
  	vsynch = 1000000000L / t1;
  
  	/* fill in Graphic device struct */
  	sprintf (pGD->modeIdent, "%dx%dx%d %ldkHz %ldHz", res_mode->xres,
  		 res_mode->yres, bits_per_pixel, (hsynch / 1000),
  		 (vsynch / 1000));
  	printf ("%s
  ", pGD->modeIdent);
  	pGD->winSizeX = res_mode->xres;
  	pGD->winSizeY = res_mode->yres;
  	pGD->plnSizeX = res_mode->xres;
  	pGD->plnSizeY = res_mode->yres;
  	switch (bits_per_pixel) {
  	case 8:
  		pGD->gdfBytesPP = 1;
  		pGD->gdfIndex = GDF__8BIT_INDEX;
  		break;
  	case 15:
  		pGD->gdfBytesPP = 2;
  		pGD->gdfIndex = GDF_15BIT_555RGB;
  		break;
  	case 16:
  		pGD->gdfBytesPP = 2;
  		pGD->gdfIndex = GDF_16BIT_565RGB;
  		break;
  	case 24:
  		pGD->gdfBytesPP = 3;
  		pGD->gdfIndex = GDF_24BIT_888RGB;
  		break;
  	}
  
  	pGD->isaBase = CONFIG_SYS_ISA_IO;
  	pGD->pciBase = pci_mem_base;
  	pGD->dprBase = (pci_mem_base + 0x400000 + 0x8000);
  	pGD->vprBase = (pci_mem_base + 0x400000 + 0xc000);
  	pGD->cprBase = (pci_mem_base + 0x400000 + 0xe000);
  	pGD->frameAdrs = pci_mem_base;
  	pGD->memSize = VIDEO_MEM_SIZE;
  
  	/* Set up hardware : select color mode,
  	   set Register base to isa 3dx for 3?x regs*/
  	out8 (SMI_MISC_REG, 0x01);
  
  	/* Turn off display */
  	smiWrite (SMI_INDX_C4, 0x01, 0x20);
  
  	/* Unlock ext. crt regs */
  	out8 (SMI_LOCK_REG, 0x40);
  
  	/* Unlock crt regs 0-7 */
  	smiWrite (SMI_INDX_D4, 0x11, 0x0e);
  
  	/* Sytem Control Register */
  	smiLoadRegs (SMI_INDX_C4, SMI_DATA_C5, SMI_SCR, sizeof(SMI_SCR));
  
  	/* extented CRT Register */
  	smiLoadRegs (SMI_INDX_D4, SMI_DATA_D5, SMI_EXT_CRT, sizeof(SMI_EXT_CRT));
  
  	/* Attributes controller registers */
  	smiLoadRegs (SMI_INDX_ATTR, SMI_INDX_ATTR, SMI_ATTR, sizeof(SMI_ATTR));
  
  	/* Graphics Controller Register */
  	smiLoadRegs (SMI_INDX_CE, SMI_DATA_CF, SMI_GCR, sizeof(SMI_GCR));
  
  	/* Sequencer Register */
  	smiLoadRegs (SMI_INDX_C4, SMI_DATA_C5, SMI_SEQR, sizeof(SMI_SEQR));
  
  	/* Power Control Register */
  	smiLoadRegs (SMI_INDX_C4, SMI_DATA_C5, SMI_PCR, sizeof(SMI_PCR));
  
  	/* Memory Control Register */
  	/* Register MSR62 is a power on configurable register. We don't */
  	/* modify it */
  	smiLoadRegs (SMI_INDX_C4, SMI_DATA_C5, SMI_MCR, sizeof(SMI_MCR));
  
  	/* Set misc output register */
  	smiLoadMsr (res_mode);
  
  	/* Set CRT and Clock control registers */
  	smiLoadCrt (res_mode, bits_per_pixel);
  
  	smiLoadCcr (res_mode, device_id);
  
  	/* Hardware Cusor Register */
  	smiLoadRegs (SMI_INDX_C4, SMI_DATA_C5, SMI_HCR, sizeof(SMI_HCR));
  
  	/* Enable  Display  */
  	videoout = 2;	    /* Default output is CRT */
  	if ((penv = getenv ("videoout")) != NULL) {
  		/* deceide if it is a string */
  		videoout = (int) simple_strtoul (penv, NULL, 16);
  	}
  	smiWrite (SMI_INDX_C4, 0x31, videoout);
  
  	/* Video processor default setup */
  	smiInitVideoProcessor ();
  
  	/* Capture port default setup */
  	smiInitCapturePort ();
  
  	/* Drawing engine default setup */
  	smiInitDrawingEngine ();
  
  	/* Turn on display */
  	smiWrite (0x3c4, 0x01, 0x01);
  
  	/* Clear video memory */
  	i = pGD->memSize/4;
  	vm = (unsigned int *)pGD->pciBase;
  	while(i--)
  		*vm++ = 0;
  	return ((void*)&smi);
  }
  
  /*******************************************************************************
   *
   * Drawing engine fill on screen region
   */
  void video_hw_rectfill (
  	unsigned int bpp,	      /* bytes per pixel */
  	unsigned int dst_x,	      /* dest pos x */
  	unsigned int dst_y,	      /* dest pos y */
  	unsigned int dim_x,	      /* frame width */
  	unsigned int dim_y,	      /* frame height */
  	unsigned int color	      /* fill color */
  	)
  {
  	register GraphicDevice *pGD = (GraphicDevice *)&smi;
  	register unsigned int control;
  
  	dim_x *= bpp;
  
  	out32r ((pGD->dprBase + 0x0014), color);
  	out32r ((pGD->dprBase + 0x0004), ((dst_x<<16) | dst_y));
  	out32r ((pGD->dprBase + 0x0008), ((dim_x<<16) | dim_y));
  
  	control = 0x0000ffff &	in32r ((pGD->dprBase + 0x000c));
  
  	control |= 0x80010000;
  
  	out32r ((pGD->dprBase + 0x000c),  control);
  
  	/* Wait for drawing processor */
  	do
  	{
  		out8 ((pGD->isaBase + 0x3c4), 0x16);
  	} while (in8 (pGD->isaBase + 0x3c5) & 0x08);
  }
  
  /*******************************************************************************
   *
   * Drawing engine bitblt with screen region
   */
  void video_hw_bitblt (
  	unsigned int bpp,	      /* bytes per pixel */
  	unsigned int src_x,	      /* source pos x */
  	unsigned int src_y,	      /* source pos y */
  	unsigned int dst_x,	      /* dest pos x */
  	unsigned int dst_y,	      /* dest pos y */
  	unsigned int dim_x,	      /* frame width */
  	unsigned int dim_y	      /* frame height */
  	)
  {
  	register GraphicDevice *pGD = (GraphicDevice *)&smi;
  	register unsigned int control;
  
  	dim_x *= bpp;
  
  	if ((src_y<dst_y) || ((src_y==dst_y) && (src_x<dst_x)))
  	{
  		out32r ((pGD->dprBase + 0x0000), (((src_x+dim_x-1)<<16) | (src_y+dim_y-1)));
  		out32r ((pGD->dprBase + 0x0004), (((dst_x+dim_x-1)<<16) | (dst_y+dim_y-1)));
  		control = 0x88000000;
  	} else {
  		out32r ((pGD->dprBase + 0x0000), ((src_x<<16) | src_y));
  		out32r ((pGD->dprBase + 0x0004), ((dst_x<<16) | dst_y));
  		control = 0x80000000;
  	}
  
  	out32r ((pGD->dprBase + 0x0008), ((dim_x<<16) | dim_y));
  	control |= (0x0000ffff &  in32r ((pGD->dprBase + 0x000c)));
  	out32r ((pGD->dprBase + 0x000c), control);
  
  	/* Wait for drawing processor */
  	do
  	{
  		out8 ((pGD->isaBase + 0x3c4), 0x16);
  	} while (in8 (pGD->isaBase + 0x3c5) & 0x08);
  }
  
  /*******************************************************************************
   *
   * Set a RGB color in the LUT (8 bit index)
   */
  void video_set_lut (
  	unsigned int index,	      /* color number */
  	unsigned char r,	      /* red */
  	unsigned char g,	      /* green */
  	unsigned char b		      /* blue */
  	)
  {
  	register GraphicDevice *pGD = (GraphicDevice *)&smi;
  
  	out8 (SMI_LUT_MASK,  0xff);
  
  	out8 (SMI_LUT_START, (char)index);
  
  	out8 (SMI_LUT_RGB, r>>2);    /* red */
  	udelay (10);
  	out8 (SMI_LUT_RGB, g>>2);    /* green */
  	udelay (10);
  	out8 (SMI_LUT_RGB, b>>2);    /* blue */
  	udelay (10);
  }