From 9ca8dbcc65cfc63d6f5ef3312a33184e1d726e00 Mon Sep 17 00:00:00 2001
From: Yunhong Jiang <yunhong.jiang@intel.com>
Date: Tue, 4 Aug 2015 12:17:53 -0700
Subject: Add the rt linux 4.1.3-rt3 as base

Import the rt linux 4.1.3-rt3 as OPNFV kvm base.

It's from git://git.kernel.org/pub/scm/linux/kernel/git/rt/linux-rt-devel.git linux-4.1.y-rt and
the base is:

commit 0917f823c59692d751951bf5ea699a2d1e2f26a2
Author: Sebastian Andrzej Siewior <bigeasy@linutronix.de>
Date:   Sat Jul 25 12:13:34 2015 +0200

    Prepare v4.1.3-rt3

    Signed-off-by: Sebastian Andrzej Siewior <bigeasy@linutronix.de>

We lose all the git history this way and it's not good. We
should apply another opnfv project repo in future.

Change-Id: I87543d81c9df70d99c5001fbdf646b202c19f423
Signed-off-by: Yunhong Jiang <yunhong.jiang@intel.com>
---
 kernel/drivers/video/fbdev/kyro/Makefile           |   8 +
 .../drivers/video/fbdev/kyro/STG4000InitDevice.c   | 326 +++++++++
 kernel/drivers/video/fbdev/kyro/STG4000Interface.h |  61 ++
 .../video/fbdev/kyro/STG4000OverlayDevice.c        | 601 +++++++++++++++
 kernel/drivers/video/fbdev/kyro/STG4000Ramdac.c    | 163 +++++
 kernel/drivers/video/fbdev/kyro/STG4000Reg.h       | 283 ++++++++
 kernel/drivers/video/fbdev/kyro/STG4000VTG.c       | 170 +++++
 kernel/drivers/video/fbdev/kyro/fbdev.c            | 808 +++++++++++++++++++++
 8 files changed, 2420 insertions(+)
 create mode 100644 kernel/drivers/video/fbdev/kyro/Makefile
 create mode 100644 kernel/drivers/video/fbdev/kyro/STG4000InitDevice.c
 create mode 100644 kernel/drivers/video/fbdev/kyro/STG4000Interface.h
 create mode 100644 kernel/drivers/video/fbdev/kyro/STG4000OverlayDevice.c
 create mode 100644 kernel/drivers/video/fbdev/kyro/STG4000Ramdac.c
 create mode 100644 kernel/drivers/video/fbdev/kyro/STG4000Reg.h
 create mode 100644 kernel/drivers/video/fbdev/kyro/STG4000VTG.c
 create mode 100644 kernel/drivers/video/fbdev/kyro/fbdev.c

(limited to 'kernel/drivers/video/fbdev/kyro')

diff --git a/kernel/drivers/video/fbdev/kyro/Makefile b/kernel/drivers/video/fbdev/kyro/Makefile
new file mode 100644
index 000000000..2fd66f551
--- /dev/null
+++ b/kernel/drivers/video/fbdev/kyro/Makefile
@@ -0,0 +1,8 @@
+#
+# Makefile for the Kyro framebuffer driver
+#
+
+obj-$(CONFIG_FB_KYRO)	+= kyrofb.o
+
+kyrofb-objs	:= STG4000Ramdac.o STG4000VTG.o STG4000OverlayDevice.o \
+		   STG4000InitDevice.o fbdev.o
diff --git a/kernel/drivers/video/fbdev/kyro/STG4000InitDevice.c b/kernel/drivers/video/fbdev/kyro/STG4000InitDevice.c
new file mode 100644
index 000000000..1d3f2080a
--- /dev/null
+++ b/kernel/drivers/video/fbdev/kyro/STG4000InitDevice.c
@@ -0,0 +1,326 @@
+/*
+ *  linux/drivers/video/kyro/STG4000InitDevice.c
+ *
+ *  Copyright (C) 2000 Imagination Technologies Ltd
+ *  Copyright (C) 2002 STMicroelectronics
+ *
+ * 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 <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/types.h>
+#include <linux/pci.h>
+
+#include "STG4000Reg.h"
+#include "STG4000Interface.h"
+
+/* SDRAM fixed settings */
+#define SDRAM_CFG_0   0x49A1
+#define SDRAM_CFG_1   0xA732
+#define SDRAM_CFG_2   0x31
+#define SDRAM_ARB_CFG 0xA0
+#define SDRAM_REFRESH 0x20
+
+/* Reset values */
+#define PMX2_SOFTRESET_DAC_RST		0x0001
+#define PMX2_SOFTRESET_C1_RST		0x0004
+#define PMX2_SOFTRESET_C2_RST		0x0008
+#define PMX2_SOFTRESET_3D_RST		0x0010
+#define PMX2_SOFTRESET_VIDIN_RST	0x0020
+#define PMX2_SOFTRESET_TLB_RST		0x0040
+#define PMX2_SOFTRESET_SD_RST		0x0080
+#define PMX2_SOFTRESET_VGA_RST		0x0100
+#define PMX2_SOFTRESET_ROM_RST		0x0200	/* reserved bit, do not reset */
+#define PMX2_SOFTRESET_TA_RST		0x0400
+#define PMX2_SOFTRESET_REG_RST		0x4000
+#define PMX2_SOFTRESET_ALL		0x7fff
+
+/* Core clock freq */
+#define CORE_PLL_FREQ 1000000
+
+/* Reference Clock freq */
+#define REF_FREQ 14318
+
+/* PCI Registers */
+static u16 CorePllControl = 0x70;
+
+#define	PCI_CONFIG_SUBSYS_ID	0x2e
+
+/* Misc */
+#define CORE_PLL_MODE_REG_0_7      3
+#define CORE_PLL_MODE_REG_8_15     2
+#define CORE_PLL_MODE_CONFIG_REG   1
+#define DAC_PLL_CONFIG_REG         0
+
+#define STG_MAX_VCO 500000
+#define STG_MIN_VCO 100000
+
+/* PLL Clock */
+#define    STG4K3_PLL_SCALER      8	/* scale numbers by 2^8 for fixed point calc */
+#define    STG4K3_PLL_MIN_R       2	/* Minimum multiplier */
+#define    STG4K3_PLL_MAX_R       33	/* Max */
+#define    STG4K3_PLL_MIN_F       2	/* Minimum divisor */
+#define    STG4K3_PLL_MAX_F       513	/* Max */
+#define    STG4K3_PLL_MIN_OD      0	/* Min output divider (shift) */
+#define    STG4K3_PLL_MAX_OD      2	/* Max */
+#define    STG4K3_PLL_MIN_VCO_SC  (100000000 >> STG4K3_PLL_SCALER)	/* Min VCO rate */
+#define    STG4K3_PLL_MAX_VCO_SC  (500000000 >> STG4K3_PLL_SCALER)	/* Max VCO rate */
+#define    STG4K3_PLL_MINR_VCO_SC (100000000 >> STG4K3_PLL_SCALER)	/* Min VCO rate (restricted) */
+#define    STG4K3_PLL_MAXR_VCO_SC (500000000 >> STG4K3_PLL_SCALER)	/* Max VCO rate (restricted) */
+#define    STG4K3_PLL_MINR_VCO    100000000	/* Min VCO rate (restricted) */
+#define    STG4K3_PLL_MAX_VCO     500000000	/* Max VCO rate */
+#define    STG4K3_PLL_MAXR_VCO    500000000	/* Max VCO rate (restricted) */
+
+#define OS_DELAY(X) \
+{ \
+volatile u32 i,count=0; \
+    for(i=0;i<X;i++) count++; \
+}
+
+static u32 InitSDRAMRegisters(volatile STG4000REG __iomem *pSTGReg,
+			      u32 dwSubSysID, u32 dwRevID)
+{
+	u32 adwSDRAMArgCfg0[] = { 0xa0, 0x80, 0xa0, 0xa0, 0xa0 };
+	u32 adwSDRAMCfg1[] = { 0x8732, 0x8732, 0xa732, 0xa732, 0x8732 };
+	u32 adwSDRAMCfg2[] = { 0x87d2, 0x87d2, 0xa7d2, 0x87d2, 0xa7d2 };
+	u32 adwSDRAMRsh[] = { 36, 39, 40 };
+	u32 adwChipSpeed[] = { 110, 120, 125 };
+	u32 dwMemTypeIdx;
+	u32 dwChipSpeedIdx;
+
+	/* Get memory tpye and chip speed indexs from the SubSysDevID */
+	dwMemTypeIdx = (dwSubSysID & 0x70) >> 4;
+	dwChipSpeedIdx = (dwSubSysID & 0x180) >> 7;
+
+	if (dwMemTypeIdx > 4 || dwChipSpeedIdx > 2)
+		return 0;
+
+	/* Program SD-RAM interface */
+	STG_WRITE_REG(SDRAMArbiterConf, adwSDRAMArgCfg0[dwMemTypeIdx]);
+	if (dwRevID < 5) {
+		STG_WRITE_REG(SDRAMConf0, 0x49A1);
+		STG_WRITE_REG(SDRAMConf1, adwSDRAMCfg1[dwMemTypeIdx]);
+	} else {
+		STG_WRITE_REG(SDRAMConf0, 0x4DF1);
+		STG_WRITE_REG(SDRAMConf1, adwSDRAMCfg2[dwMemTypeIdx]);
+	}
+
+	STG_WRITE_REG(SDRAMConf2, 0x31);
+	STG_WRITE_REG(SDRAMRefresh, adwSDRAMRsh[dwChipSpeedIdx]);
+
+	return adwChipSpeed[dwChipSpeedIdx] * 10000;
+}
+
+u32 ProgramClock(u32 refClock,
+		   u32 coreClock,
+		   u32 * FOut, u32 * ROut, u32 * POut)
+{
+	u32 R = 0, F = 0, OD = 0, ODIndex = 0;
+	u32 ulBestR = 0, ulBestF = 0, ulBestOD = 0;
+	u32 ulBestVCO = 0, ulBestClk = 0, ulBestScore = 0;
+	u32 ulScore, ulPhaseScore, ulVcoScore;
+	u32 ulTmp = 0, ulVCO;
+	u32 ulScaleClockReq, ulMinClock, ulMaxClock;
+	u32 ODValues[] = { 1, 2, 0 };
+
+	/* Translate clock in Hz */
+	coreClock *= 100;	/* in Hz */
+	refClock *= 1000;	/* in Hz */
+
+	/* Work out acceptable clock
+	 * The method calculates ~ +- 0.4% (1/256)
+	 */
+	ulMinClock = coreClock - (coreClock >> 8);
+	ulMaxClock = coreClock + (coreClock >> 8);
+
+	/* Scale clock required for use in calculations */
+	ulScaleClockReq = coreClock >> STG4K3_PLL_SCALER;
+
+	/* Iterate through post divider values */
+	for (ODIndex = 0; ODIndex < 3; ODIndex++) {
+		OD = ODValues[ODIndex];
+		R = STG4K3_PLL_MIN_R;
+
+		/* loop for pre-divider from min to max  */
+		while (R <= STG4K3_PLL_MAX_R) {
+			/* estimate required feedback multiplier */
+			ulTmp = R * (ulScaleClockReq << OD);
+
+			/* F = ClkRequired * R * (2^OD) / Fref */
+			F = (u32)(ulTmp / (refClock >> STG4K3_PLL_SCALER));
+
+			/* compensate for accuracy */
+			if (F > STG4K3_PLL_MIN_F)
+				F--;
+
+
+			/*
+			 * We should be close to our target frequency (if it's
+			 * achievable with current OD & R) let's iterate
+			 * through F for best fit
+			 */
+			while ((F >= STG4K3_PLL_MIN_F) &&
+			       (F <= STG4K3_PLL_MAX_F)) {
+				/* Calc VCO at full accuracy */
+				ulVCO = refClock / R;
+				ulVCO = F * ulVCO;
+
+				/*
+				 * Check it's within restricted VCO range
+				 * unless of course the desired frequency is
+				 * above the restricted range, then test
+				 * against VCO limit
+				 */
+				if ((ulVCO >= STG4K3_PLL_MINR_VCO) &&
+				    ((ulVCO <= STG4K3_PLL_MAXR_VCO) ||
+				     ((coreClock > STG4K3_PLL_MAXR_VCO)
+				      && (ulVCO <= STG4K3_PLL_MAX_VCO)))) {
+					ulTmp = (ulVCO >> OD);	/* Clock = VCO / (2^OD) */
+
+					/* Is this clock good enough? */
+					if ((ulTmp >= ulMinClock)
+					    && (ulTmp <= ulMaxClock)) {
+						ulPhaseScore = (((refClock / R) - (refClock / STG4K3_PLL_MAX_R))) / ((refClock - (refClock / STG4K3_PLL_MAX_R)) >> 10);
+
+						ulVcoScore = ((ulVCO - STG4K3_PLL_MINR_VCO)) / ((STG4K3_PLL_MAXR_VCO - STG4K3_PLL_MINR_VCO) >> 10);
+						ulScore = ulPhaseScore + ulVcoScore;
+
+						if (!ulBestScore) {
+							ulBestVCO = ulVCO;
+							ulBestOD = OD;
+							ulBestF = F;
+							ulBestR = R;
+							ulBestClk = ulTmp;
+							ulBestScore =
+							    ulScore;
+						}
+						/* is this better, ( aim for highest Score) */
+			/*--------------------------------------------------------------------------
+                             Here we want to use a scoring system which will take account of both the
+                            value at the phase comparater and the VCO output
+                             to do this we will use a cumulative score between the two
+                          The way this ends up is that we choose the first value in the loop anyway
+                          but we shall keep this code in case new restrictions come into play
+                          --------------------------------------------------------------------------*/
+						if ((ulScore >= ulBestScore) && (OD > 0)) {
+							ulBestVCO = ulVCO;
+							ulBestOD = OD;
+							ulBestF = F;
+							ulBestR = R;
+							ulBestClk = ulTmp;
+							ulBestScore =
+							    ulScore;
+						}
+					}
+				}
+				F++;
+			}
+			R++;
+		}
+	}
+
+	/*
+	   did we find anything?
+	   Then return RFOD
+	 */
+	if (ulBestScore) {
+		*ROut = ulBestR;
+		*FOut = ulBestF;
+
+		if ((ulBestOD == 2) || (ulBestOD == 3)) {
+			*POut = 3;
+		} else
+			*POut = ulBestOD;
+
+	}
+
+	return (ulBestClk);
+}
+
+int SetCoreClockPLL(volatile STG4000REG __iomem *pSTGReg, struct pci_dev *pDev)
+{
+	u32 F, R, P;
+	u16 core_pll = 0, sub;
+	u32 ulCoreClock;
+	u32 tmp;
+	u32 ulChipSpeed;
+
+	STG_WRITE_REG(IntMask, 0xFFFF);
+
+	/* Disable Primary Core Thread0 */
+	tmp = STG_READ_REG(Thread0Enable);
+	CLEAR_BIT(0);
+	STG_WRITE_REG(Thread0Enable, tmp);
+
+	/* Disable Primary Core Thread1 */
+	tmp = STG_READ_REG(Thread1Enable);
+	CLEAR_BIT(0);
+	STG_WRITE_REG(Thread1Enable, tmp);
+
+	STG_WRITE_REG(SoftwareReset,
+		      PMX2_SOFTRESET_REG_RST | PMX2_SOFTRESET_ROM_RST);
+	STG_WRITE_REG(SoftwareReset,
+		      PMX2_SOFTRESET_REG_RST | PMX2_SOFTRESET_TA_RST |
+		      PMX2_SOFTRESET_ROM_RST);
+
+	/* Need to play around to reset TA */
+	STG_WRITE_REG(TAConfiguration, 0);
+	STG_WRITE_REG(SoftwareReset,
+		      PMX2_SOFTRESET_REG_RST | PMX2_SOFTRESET_ROM_RST);
+	STG_WRITE_REG(SoftwareReset,
+		      PMX2_SOFTRESET_REG_RST | PMX2_SOFTRESET_TA_RST |
+		      PMX2_SOFTRESET_ROM_RST);
+
+	pci_read_config_word(pDev, PCI_CONFIG_SUBSYS_ID, &sub);
+
+	ulChipSpeed = InitSDRAMRegisters(pSTGReg, (u32)sub,
+		                         (u32)pDev->revision);
+
+	if (ulChipSpeed == 0)
+		return -EINVAL;
+
+	ulCoreClock = ProgramClock(REF_FREQ, CORE_PLL_FREQ, &F, &R, &P);
+
+	core_pll |= ((P) | ((F - 2) << 2) | ((R - 2) << 11));
+
+	/* Set Core PLL Control to Core PLL Mode  */
+
+	/* Send bits 0:7 of the Core PLL Mode register */
+	tmp = ((CORE_PLL_MODE_REG_0_7 << 8) | (core_pll & 0x00FF));
+	pci_write_config_word(pDev, CorePllControl, tmp);
+	/* Without some delay between the PCI config writes the clock does
+	   not reliably set when the code is compiled -O3
+	 */
+	OS_DELAY(1000000);
+
+	tmp |= SET_BIT(14);
+	pci_write_config_word(pDev, CorePllControl, tmp);
+	OS_DELAY(1000000);
+
+	/* Send bits 8:15 of the Core PLL Mode register */
+	tmp =
+	    ((CORE_PLL_MODE_REG_8_15 << 8) | ((core_pll & 0xFF00) >> 8));
+	pci_write_config_word(pDev, CorePllControl, tmp);
+	OS_DELAY(1000000);
+
+	tmp |= SET_BIT(14);
+	pci_write_config_word(pDev, CorePllControl, tmp);
+	OS_DELAY(1000000);
+
+	STG_WRITE_REG(SoftwareReset, PMX2_SOFTRESET_ALL);
+
+#if 0
+	/* Enable Primary Core Thread0 */
+	tmp = ((STG_READ_REG(Thread0Enable)) | SET_BIT(0));
+	STG_WRITE_REG(Thread0Enable, tmp);
+
+	/* Enable Primary Core Thread1 */
+	tmp = ((STG_READ_REG(Thread1Enable)) | SET_BIT(0));
+	STG_WRITE_REG(Thread1Enable, tmp);
+#endif
+
+	return 0;
+}
diff --git a/kernel/drivers/video/fbdev/kyro/STG4000Interface.h b/kernel/drivers/video/fbdev/kyro/STG4000Interface.h
new file mode 100644
index 000000000..b7c83d5df
--- /dev/null
+++ b/kernel/drivers/video/fbdev/kyro/STG4000Interface.h
@@ -0,0 +1,61 @@
+/*
+ *  linux/drivers/video/kyro/STG4000Interface.h
+ *
+ *  Copyright (C) 2002 STMicroelectronics
+ *
+ * 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.
+ */
+
+#ifndef _STG4000INTERFACE_H
+#define _STG4000INTERFACE_H
+
+#include <linux/pci.h>
+#include <video/kyro.h>
+
+/*
+ * Ramdac Setup
+ */
+extern int InitialiseRamdac(volatile STG4000REG __iomem *pSTGReg, u32 displayDepth,
+			    u32 displayWidth, u32 displayHeight,
+			    s32 HSyncPolarity, s32 VSyncPolarity,
+			    u32 *pixelClock);
+
+extern void DisableRamdacOutput(volatile STG4000REG __iomem *pSTGReg);
+extern void EnableRamdacOutput(volatile STG4000REG __iomem *pSTGReg);
+
+/*
+ * Timing generator setup
+ */
+extern void DisableVGA(volatile STG4000REG __iomem *pSTGReg);
+extern void StopVTG(volatile STG4000REG __iomem *pSTGReg);
+extern void StartVTG(volatile STG4000REG __iomem *pSTGReg);
+extern void SetupVTG(volatile STG4000REG __iomem *pSTGReg,
+		     const struct kyrofb_info * pTiming);
+
+extern u32 ProgramClock(u32 refClock, u32 coreClock, u32 *FOut, u32 *ROut, u32 *POut);
+extern int SetCoreClockPLL(volatile STG4000REG __iomem *pSTGReg, struct pci_dev *pDev);
+
+/*
+ * Overlay setup
+ */
+extern void ResetOverlayRegisters(volatile STG4000REG __iomem *pSTGReg);
+
+extern int CreateOverlaySurface(volatile STG4000REG __iomem *pSTGReg,
+				u32 ulWidth, u32 ulHeight,
+				int bLinear,
+				u32 ulOverlayOffset,
+				u32 * retStride, u32 * retUVStride);
+
+extern int SetOverlayBlendMode(volatile STG4000REG __iomem *pSTGReg,
+			       OVRL_BLEND_MODE mode,
+			       u32 ulAlpha, u32 ulColorKey);
+
+extern int SetOverlayViewPort(volatile STG4000REG __iomem *pSTGReg,
+			      u32 left, u32 top,
+			      u32 right, u32 bottom);
+
+extern void EnableOverlayPlane(volatile STG4000REG __iomem *pSTGReg);
+
+#endif /* _STG4000INTERFACE_H */
diff --git a/kernel/drivers/video/fbdev/kyro/STG4000OverlayDevice.c b/kernel/drivers/video/fbdev/kyro/STG4000OverlayDevice.c
new file mode 100644
index 000000000..0aeeaa107
--- /dev/null
+++ b/kernel/drivers/video/fbdev/kyro/STG4000OverlayDevice.c
@@ -0,0 +1,601 @@
+/*
+ *  linux/drivers/video/kyro/STG4000OverlayDevice.c
+ *
+ *  Copyright (C) 2000 Imagination Technologies Ltd
+ *  Copyright (C) 2002 STMicroelectronics
+ *
+ * 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 <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/types.h>
+
+#include "STG4000Reg.h"
+#include "STG4000Interface.h"
+
+/* HW Defines */
+
+#define STG4000_NO_SCALING    0x800
+#define STG4000_NO_DECIMATION 0xFFFFFFFF
+
+/* Primary surface */
+#define STG4000_PRIM_NUM_PIX   5
+#define STG4000_PRIM_ALIGN     4
+#define STG4000_PRIM_ADDR_BITS 20
+
+#define STG4000_PRIM_MIN_WIDTH  640
+#define STG4000_PRIM_MAX_WIDTH  1600
+#define STG4000_PRIM_MIN_HEIGHT 480
+#define STG4000_PRIM_MAX_HEIGHT 1200
+
+/* Overlay surface */
+#define STG4000_OVRL_NUM_PIX   4
+#define STG4000_OVRL_ALIGN     2
+#define STG4000_OVRL_ADDR_BITS 20
+#define STG4000_OVRL_NUM_MODES 5
+
+#define STG4000_OVRL_MIN_WIDTH  0
+#define STG4000_OVRL_MAX_WIDTH  720
+#define STG4000_OVRL_MIN_HEIGHT 0
+#define STG4000_OVRL_MAX_HEIGHT 576
+
+/* Decimation and Scaling */
+static u32 adwDecim8[33] = {
+	    0xffffffff, 0xfffeffff, 0xffdffbff, 0xfefefeff, 0xfdf7efbf,
+	    0xfbdf7bdf, 0xf7bbddef, 0xeeeeeeef, 0xeeddbb77, 0xedb76db7,
+	    0xdb6db6db, 0xdb5b5b5b, 0xdab5ad6b, 0xd5ab55ab, 0xd555aaab,
+	    0xaaaaaaab, 0xaaaa5555, 0xaa952a55, 0xa94a5295, 0xa5252525,
+	    0xa4924925, 0x92491249, 0x91224489, 0x91111111, 0x90884211,
+	    0x88410821, 0x88102041, 0x81010101, 0x80800801, 0x80010001,
+	    0x80000001, 0x00000001, 0x00000000
+};
+
+typedef struct _OVRL_SRC_DEST {
+	/*clipped on-screen pixel position of overlay */
+	u32 ulDstX1;
+	u32 ulDstY1;
+	u32 ulDstX2;
+	u32 ulDstY2;
+
+	/*clipped pixel pos of source data within buffer thses need to be 128 bit word aligned */
+	u32 ulSrcX1;
+	u32 ulSrcY1;
+	u32 ulSrcX2;
+	u32 ulSrcY2;
+
+	/* on-screen pixel position of overlay */
+	s32 lDstX1;
+	s32 lDstY1;
+	s32 lDstX2;
+	s32 lDstY2;
+} OVRL_SRC_DEST;
+
+static u32 ovlWidth, ovlHeight, ovlStride;
+static int ovlLinear;
+
+void ResetOverlayRegisters(volatile STG4000REG __iomem *pSTGReg)
+{
+	u32 tmp;
+
+	/* Set Overlay address to default */
+	tmp = STG_READ_REG(DACOverlayAddr);
+	CLEAR_BITS_FRM_TO(0, 20);
+	CLEAR_BIT(31);
+	STG_WRITE_REG(DACOverlayAddr, tmp);
+
+	/* Set Overlay U address */
+	tmp = STG_READ_REG(DACOverlayUAddr);
+	CLEAR_BITS_FRM_TO(0, 20);
+	STG_WRITE_REG(DACOverlayUAddr, tmp);
+
+	/* Set Overlay V address */
+	tmp = STG_READ_REG(DACOverlayVAddr);
+	CLEAR_BITS_FRM_TO(0, 20);
+	STG_WRITE_REG(DACOverlayVAddr, tmp);
+
+	/* Set Overlay Size */
+	tmp = STG_READ_REG(DACOverlaySize);
+	CLEAR_BITS_FRM_TO(0, 10);
+	CLEAR_BITS_FRM_TO(12, 31);
+	STG_WRITE_REG(DACOverlaySize, tmp);
+
+	/* Set Overlay Vt Decimation */
+	tmp = STG4000_NO_DECIMATION;
+	STG_WRITE_REG(DACOverlayVtDec, tmp);
+
+	/* Set Overlay format to default value */
+	tmp = STG_READ_REG(DACPixelFormat);
+	CLEAR_BITS_FRM_TO(4, 7);
+	CLEAR_BITS_FRM_TO(16, 22);
+	STG_WRITE_REG(DACPixelFormat, tmp);
+
+	/* Set Vertical scaling to default */
+	tmp = STG_READ_REG(DACVerticalScal);
+	CLEAR_BITS_FRM_TO(0, 11);
+	CLEAR_BITS_FRM_TO(16, 22);
+	tmp |= STG4000_NO_SCALING;	/* Set to no scaling */
+	STG_WRITE_REG(DACVerticalScal, tmp);
+
+	/* Set Horizontal Scaling to default */
+	tmp = STG_READ_REG(DACHorizontalScal);
+	CLEAR_BITS_FRM_TO(0, 11);
+	CLEAR_BITS_FRM_TO(16, 17);
+	tmp |= STG4000_NO_SCALING;	/* Set to no scaling */
+	STG_WRITE_REG(DACHorizontalScal, tmp);
+
+	/* Set Blend mode to Alpha Blend */
+	/* ????? SG 08/11/2001 Surely this isn't the alpha blend mode,
+	   hopefully its overwrite
+	 */
+	tmp = STG_READ_REG(DACBlendCtrl);
+	CLEAR_BITS_FRM_TO(0, 30);
+	tmp = (GRAPHICS_MODE << 28);
+	STG_WRITE_REG(DACBlendCtrl, tmp);
+
+}
+
+int CreateOverlaySurface(volatile STG4000REG __iomem *pSTGReg,
+			 u32 inWidth,
+			 u32 inHeight,
+			 int bLinear,
+			 u32 ulOverlayOffset,
+			 u32 * retStride, u32 * retUVStride)
+{
+	u32 tmp;
+	u32 ulStride;
+
+	if (inWidth > STG4000_OVRL_MAX_WIDTH ||
+	    inHeight > STG4000_OVRL_MAX_HEIGHT) {
+		return -EINVAL;
+	}
+
+	/* Stride in 16 byte words - 16Bpp */
+	if (bLinear) {
+		/* Format is 16bits so num 16 byte words is width/8 */
+		if ((inWidth & 0x7) == 0) {	/* inWidth % 8 */
+			ulStride = (inWidth / 8);
+		} else {
+			/* Round up to next 16byte boundary */
+			ulStride = ((inWidth + 8) / 8);
+		}
+	} else {
+		/* Y component is 8bits so num 16 byte words is width/16 */
+		if ((inWidth & 0xf) == 0) {	/* inWidth % 16 */
+			ulStride = (inWidth / 16);
+		} else {
+			/* Round up to next 16byte boundary */
+			ulStride = ((inWidth + 16) / 16);
+		}
+	}
+
+
+	/* Set Overlay address and Format mode */
+	tmp = STG_READ_REG(DACOverlayAddr);
+	CLEAR_BITS_FRM_TO(0, 20);
+	if (bLinear) {
+		CLEAR_BIT(31);	/* Overlay format to Linear */
+	} else {
+		tmp |= SET_BIT(31);	/* Overlay format to Planer */
+	}
+
+	/* Only bits 24:4 of the Overlay address */
+	tmp |= (ulOverlayOffset >> 4);
+	STG_WRITE_REG(DACOverlayAddr, tmp);
+
+	if (!bLinear) {
+		u32 uvSize =
+		    (inWidth & 0x1) ? (inWidth + 1 / 2) : (inWidth / 2);
+		u32 uvStride;
+		u32 ulOffset;
+		/* Y component is 8bits so num 32 byte words is width/32 */
+		if ((uvSize & 0xf) == 0) {	/* inWidth % 16 */
+			uvStride = (uvSize / 16);
+		} else {
+			/* Round up to next 32byte boundary */
+			uvStride = ((uvSize + 16) / 16);
+		}
+
+		ulOffset = ulOverlayOffset + (inHeight * (ulStride * 16));
+		/* Align U,V data to 32byte boundary */
+		if ((ulOffset & 0x1f) != 0)
+			ulOffset = (ulOffset + 32L) & 0xffffffE0L;
+
+		tmp = STG_READ_REG(DACOverlayUAddr);
+		CLEAR_BITS_FRM_TO(0, 20);
+		tmp |= (ulOffset >> 4);
+		STG_WRITE_REG(DACOverlayUAddr, tmp);
+
+		ulOffset += (inHeight / 2) * (uvStride * 16);
+		/* Align U,V data to 32byte boundary */
+		if ((ulOffset & 0x1f) != 0)
+			ulOffset = (ulOffset + 32L) & 0xffffffE0L;
+
+		tmp = STG_READ_REG(DACOverlayVAddr);
+		CLEAR_BITS_FRM_TO(0, 20);
+		tmp |= (ulOffset >> 4);
+		STG_WRITE_REG(DACOverlayVAddr, tmp);
+
+		*retUVStride = uvStride * 16;
+	}
+
+
+	/* Set Overlay YUV pixel format
+	 * Make sure that LUT not used - ??????
+	 */
+	tmp = STG_READ_REG(DACPixelFormat);
+	/* Only support Planer or UYVY linear formats */
+	CLEAR_BITS_FRM_TO(4, 9);
+	STG_WRITE_REG(DACPixelFormat, tmp);
+
+	ovlWidth = inWidth;
+	ovlHeight = inHeight;
+	ovlStride = ulStride;
+	ovlLinear = bLinear;
+	*retStride = ulStride << 4;	/* In bytes */
+
+	return 0;
+}
+
+int SetOverlayBlendMode(volatile STG4000REG __iomem *pSTGReg,
+			OVRL_BLEND_MODE mode,
+			u32 ulAlpha, u32 ulColorKey)
+{
+	u32 tmp;
+
+	tmp = STG_READ_REG(DACBlendCtrl);
+	CLEAR_BITS_FRM_TO(28, 30);
+	tmp |= (mode << 28);
+
+	switch (mode) {
+	case COLOR_KEY:
+		CLEAR_BITS_FRM_TO(0, 23);
+		tmp |= (ulColorKey & 0x00FFFFFF);
+		break;
+
+	case GLOBAL_ALPHA:
+		CLEAR_BITS_FRM_TO(24, 27);
+		tmp |= ((ulAlpha & 0xF) << 24);
+		break;
+
+	case CK_PIXEL_ALPHA:
+		CLEAR_BITS_FRM_TO(0, 23);
+		tmp |= (ulColorKey & 0x00FFFFFF);
+		break;
+
+	case CK_GLOBAL_ALPHA:
+		CLEAR_BITS_FRM_TO(0, 23);
+		tmp |= (ulColorKey & 0x00FFFFFF);
+		CLEAR_BITS_FRM_TO(24, 27);
+		tmp |= ((ulAlpha & 0xF) << 24);
+		break;
+
+	case GRAPHICS_MODE:
+	case PER_PIXEL_ALPHA:
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	STG_WRITE_REG(DACBlendCtrl, tmp);
+
+	return 0;
+}
+
+void EnableOverlayPlane(volatile STG4000REG __iomem *pSTGReg)
+{
+	u32 tmp;
+	/* Enable Overlay */
+	tmp = STG_READ_REG(DACPixelFormat);
+	tmp |= SET_BIT(7);
+	STG_WRITE_REG(DACPixelFormat, tmp);
+
+	/* Set video stream control */
+	tmp = STG_READ_REG(DACStreamCtrl);
+	tmp |= SET_BIT(1);	/* video stream */
+	STG_WRITE_REG(DACStreamCtrl, tmp);
+}
+
+static u32 Overlap(u32 ulBits, u32 ulPattern)
+{
+	u32 ulCount = 0;
+
+	while (ulBits) {
+		if (!(ulPattern & 1))
+			ulCount++;
+		ulBits--;
+		ulPattern = ulPattern >> 1;
+	}
+
+	return ulCount;
+
+}
+
+int SetOverlayViewPort(volatile STG4000REG __iomem *pSTGReg,
+		       u32 left, u32 top,
+		       u32 right, u32 bottom)
+{
+	OVRL_SRC_DEST srcDest;
+
+	u32 ulSrcTop, ulSrcBottom;
+	u32 ulSrc, ulDest;
+	u32 ulFxScale, ulFxOffset;
+	u32 ulHeight, ulWidth;
+	u32 ulPattern;
+	u32 ulDecimate, ulDecimated;
+	u32 ulApplied;
+	u32 ulDacXScale, ulDacYScale;
+	u32 ulScale;
+	u32 ulLeft, ulRight;
+	u32 ulSrcLeft, ulSrcRight;
+	u32 ulScaleLeft, ulScaleRight;
+	u32 ulhDecim;
+	u32 ulsVal;
+	u32 ulVertDecFactor;
+	int bResult;
+	u32 ulClipOff = 0;
+	u32 ulBits = 0;
+	u32 ulsAdd = 0;
+	u32 tmp, ulStride;
+	u32 ulExcessPixels, ulClip, ulExtraLines;
+
+
+	srcDest.ulSrcX1 = 0;
+	srcDest.ulSrcY1 = 0;
+	srcDest.ulSrcX2 = ovlWidth - 1;
+	srcDest.ulSrcY2 = ovlHeight - 1;
+
+	srcDest.ulDstX1 = left;
+	srcDest.ulDstY1 = top;
+	srcDest.ulDstX2 = right;
+	srcDest.ulDstY2 = bottom;
+
+	srcDest.lDstX1 = srcDest.ulDstX1;
+	srcDest.lDstY1 = srcDest.ulDstY1;
+	srcDest.lDstX2 = srcDest.ulDstX2;
+	srcDest.lDstY2 = srcDest.ulDstY2;
+
+    /************* Vertical decimation/scaling ******************/
+
+	/* Get Src Top and Bottom */
+	ulSrcTop = srcDest.ulSrcY1;
+	ulSrcBottom = srcDest.ulSrcY2;
+
+	ulSrc = ulSrcBottom - ulSrcTop;
+	ulDest = srcDest.lDstY2 - srcDest.lDstY1;	/* on-screen overlay */
+
+	if (ulSrc <= 1)
+		return -EINVAL;
+
+	/* First work out the position we are to display as offset from the
+	 * source of the buffer
+	 */
+	ulFxScale = (ulDest << 11) / ulSrc;	/* fixed point scale factor */
+	ulFxOffset = (srcDest.lDstY2 - srcDest.ulDstY2) << 11;
+
+	ulSrcBottom = ulSrcBottom - (ulFxOffset / ulFxScale);
+	ulSrc = ulSrcBottom - ulSrcTop;
+	ulHeight = ulSrc;
+
+	ulDest = srcDest.ulDstY2 - (srcDest.ulDstY1 - 1);
+	ulPattern = adwDecim8[ulBits];
+
+	/* At this point ulSrc represents the input decimator */
+	if (ulSrc > ulDest) {
+		ulDecimate = ulSrc - ulDest;
+		ulBits = 0;
+		ulApplied = ulSrc / 32;
+
+		while (((ulBits * ulApplied) +
+			Overlap((ulSrc % 32),
+				adwDecim8[ulBits])) < ulDecimate)
+			ulBits++;
+
+		ulPattern = adwDecim8[ulBits];
+		ulDecimated =
+		    (ulBits * ulApplied) + Overlap((ulSrc % 32),
+						   ulPattern);
+		ulSrc = ulSrc - ulDecimated;	/* the number number of lines that will go into the scaler */
+	}
+
+	if (ulBits && (ulBits != 32)) {
+		ulVertDecFactor = (63 - ulBits) / (32 - ulBits);	/* vertical decimation factor scaled up to nearest integer */
+	} else {
+		ulVertDecFactor = 1;
+	}
+
+	ulDacYScale = ((ulSrc - 1) * 2048) / (ulDest + 1);
+
+	tmp = STG_READ_REG(DACOverlayVtDec);	/* Decimation */
+	CLEAR_BITS_FRM_TO(0, 31);
+	tmp = ulPattern;
+	STG_WRITE_REG(DACOverlayVtDec, tmp);
+
+	/***************** Horizontal decimation/scaling ***************************/
+
+	/*
+	 * Now we handle the horizontal case, this is a simplified version of
+	 * the vertical case in that we decimate by factors of 2.  as we are
+	 * working in words we should always be able to decimate by these
+	 * factors.  as we always have to have a buffer which is aligned to a
+	 * whole number of 128 bit words, we must align the left side to the
+	 * lowest to the next lowest 128 bit boundary, and the right hand edge
+	 * to the next largets boundary, (in a similar way to how we didi it in
+	 * PMX1) as the left and right hand edges are aligned to these
+	 * boundaries normally this only becomes an issue when we are chopping
+	 * of one of the sides We shall work out vertical stuff first
+	 */
+	ulSrc = srcDest.ulSrcX2 - srcDest.ulSrcX1;
+	ulDest = srcDest.lDstX2 - srcDest.lDstX1;
+#ifdef _OLDCODE
+	ulLeft = srcDest.ulDstX1;
+	ulRight = srcDest.ulDstX2;
+#else
+	if (srcDest.ulDstX1 > 2) {
+		ulLeft = srcDest.ulDstX1 + 2;
+		ulRight = srcDest.ulDstX2 + 1;
+	} else {
+		ulLeft = srcDest.ulDstX1;
+		ulRight = srcDest.ulDstX2 + 1;
+	}
+#endif
+	/* first work out the position we are to display as offset from the source of the buffer */
+	bResult = 1;
+
+	do {
+		if (ulDest == 0)
+			return -EINVAL;
+
+		/* source pixels per dest pixel <<11 */
+		ulFxScale = ((ulSrc - 1) << 11) / (ulDest);
+
+		/* then number of destination pixels out we are */
+		ulFxOffset = ulFxScale * ((srcDest.ulDstX1 - srcDest.lDstX1) + ulClipOff);
+		ulFxOffset >>= 11;
+
+		/* this replaces the code which was making a decision as to use either ulFxOffset or ulSrcX1 */
+		ulSrcLeft = srcDest.ulSrcX1 + ulFxOffset;
+
+		/* then number of destination pixels out we are */
+		ulFxOffset = ulFxScale * (srcDest.lDstX2 - srcDest.ulDstX2);
+		ulFxOffset >>= 11;
+
+		ulSrcRight = srcDest.ulSrcX2 - ulFxOffset;
+
+		/*
+		 * we must align these to our 128 bit boundaries. we shall
+		 * round down the pixel pos to the nearest 8 pixels.
+		 */
+		ulScaleLeft = ulSrcLeft;
+		ulScaleRight = ulSrcRight;
+
+		/* shift fxscale until it is in the range of the scaler */
+		ulhDecim = 0;
+		ulScale = (((ulSrcRight - ulSrcLeft) - 1) << (11 - ulhDecim)) / (ulRight - ulLeft + 2);
+
+		while (ulScale > 0x800) {
+			ulhDecim++;
+			ulScale = (((ulSrcRight - ulSrcLeft) - 1) << (11 - ulhDecim)) / (ulRight - ulLeft + 2);
+		}
+
+		/*
+		 * to try and get the best values We first try and use
+		 * src/dwdest for the scale factor, then we move onto src-1
+		 *
+		 * we want to check to see if we will need to clip data, if so
+		 * then we should clip our source so that we don't need to
+		 */
+		if (!ovlLinear) {
+			ulSrcLeft &= ~0x1f;
+
+			/*
+			 * we must align the right hand edge to the next 32
+			 * pixel` boundary, must be on a 256 boundary so u, and
+			 * v are 128 bit aligned
+			 */
+			ulSrcRight = (ulSrcRight + 0x1f) & ~0x1f;
+		} else {
+			ulSrcLeft &= ~0x7;
+
+			/*
+			 * we must align the right hand edge to the next
+			 * 8pixel` boundary
+			 */
+			ulSrcRight = (ulSrcRight + 0x7) & ~0x7;
+		}
+
+		/* this is the input size line store needs to cope with */
+		ulWidth = ulSrcRight - ulSrcLeft;
+
+		/*
+		 * use unclipped value to work out scale factror this is the
+		 * scale factor we want we shall now work out the horizonal
+		 * decimation and scaling
+		 */
+		ulsVal = ((ulWidth / 8) >> ulhDecim);
+
+		if ((ulWidth != (ulsVal << ulhDecim) * 8))
+			ulsAdd = 1;
+
+		/* input pixels to scaler; */
+		ulSrc = ulWidth >> ulhDecim;
+
+		if (ulSrc <= 2)
+			return -EINVAL;
+
+		ulExcessPixels = ((((ulScaleLeft - ulSrcLeft)) << (11 - ulhDecim)) / ulScale);
+
+		ulClip = (ulSrc << 11) / ulScale;
+		ulClip -= (ulRight - ulLeft);
+		ulClip += ulExcessPixels;
+
+		if (ulClip)
+			ulClip--;
+
+		/* We may need to do more here if we really have a HW rev < 5 */
+	} while (!bResult);
+
+	ulExtraLines = (1 << ulhDecim) * ulVertDecFactor;
+	ulExtraLines += 64;
+	ulHeight += ulExtraLines;
+
+	ulDacXScale = ulScale;
+
+
+	tmp = STG_READ_REG(DACVerticalScal);
+	CLEAR_BITS_FRM_TO(0, 11);
+	CLEAR_BITS_FRM_TO(16, 22);	/* Vertical Scaling */
+
+	/* Calculate new output line stride, this is always the number of 422
+	   words in the line buffer, so it doesn't matter if the
+	   mode is 420. Then set the vertical scale register.
+	 */
+	ulStride = (ulWidth >> (ulhDecim + 3)) + ulsAdd;
+	tmp |= ((ulStride << 16) | (ulDacYScale));	/* DAC_LS_CTRL = stride */
+	STG_WRITE_REG(DACVerticalScal, tmp);
+
+	/* Now set up the overlay size using the modified width and height
+	   from decimate and scaling calculations
+	 */
+	tmp = STG_READ_REG(DACOverlaySize);
+	CLEAR_BITS_FRM_TO(0, 10);
+	CLEAR_BITS_FRM_TO(12, 31);
+
+	if (ovlLinear) {
+		tmp |=
+		    (ovlStride | ((ulHeight + 1) << 12) |
+		     (((ulWidth / 8) - 1) << 23));
+	} else {
+		tmp |=
+		    (ovlStride | ((ulHeight + 1) << 12) |
+		     (((ulWidth / 32) - 1) << 23));
+	}
+
+	STG_WRITE_REG(DACOverlaySize, tmp);
+
+	/* Set Video Window Start */
+	tmp = ((ulLeft << 16)) | (srcDest.ulDstY1);
+	STG_WRITE_REG(DACVidWinStart, tmp);
+
+	/* Set Video Window End */
+	tmp = ((ulRight) << 16) | (srcDest.ulDstY2);
+	STG_WRITE_REG(DACVidWinEnd, tmp);
+
+	/* Finally set up the rest of the overlay regs in the order
+	   done in the IMG driver
+	 */
+	tmp = STG_READ_REG(DACPixelFormat);
+	tmp = ((ulExcessPixels << 16) | tmp) & 0x7fffffff;
+	STG_WRITE_REG(DACPixelFormat, tmp);
+
+	tmp = STG_READ_REG(DACHorizontalScal);
+	CLEAR_BITS_FRM_TO(0, 11);
+	CLEAR_BITS_FRM_TO(16, 17);
+	tmp |= ((ulhDecim << 16) | (ulDacXScale));
+	STG_WRITE_REG(DACHorizontalScal, tmp);
+
+	return 0;
+}
diff --git a/kernel/drivers/video/fbdev/kyro/STG4000Ramdac.c b/kernel/drivers/video/fbdev/kyro/STG4000Ramdac.c
new file mode 100644
index 000000000..e6ad037e4
--- /dev/null
+++ b/kernel/drivers/video/fbdev/kyro/STG4000Ramdac.c
@@ -0,0 +1,163 @@
+/*
+ *  linux/drivers/video/kyro/STG4000Ramdac.c
+ *
+ *  Copyright (C) 2002 STMicroelectronics
+ *
+ * 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 <linux/kernel.h>
+#include <linux/errno.h>
+#include <linux/types.h>
+#include <video/kyro.h>
+
+#include "STG4000Reg.h"
+#include "STG4000Interface.h"
+
+static u32 STG_PIXEL_BUS_WIDTH = 128;	/* 128 bit bus width      */
+static u32 REF_CLOCK = 14318;
+
+int InitialiseRamdac(volatile STG4000REG __iomem * pSTGReg,
+		     u32 displayDepth,
+		     u32 displayWidth,
+		     u32 displayHeight,
+		     s32 HSyncPolarity,
+		     s32 VSyncPolarity, u32 * pixelClock)
+{
+	u32 tmp = 0;
+	u32 F = 0, R = 0, P = 0;
+	u32 stride = 0;
+	u32 ulPdiv = 0;
+	u32 physicalPixelDepth = 0;
+	/* Make sure DAC is in Reset */
+	tmp = STG_READ_REG(SoftwareReset);
+
+	if (tmp & 0x1) {
+		CLEAR_BIT(1);
+		STG_WRITE_REG(SoftwareReset, tmp);
+	}
+
+	/* Set Pixel Format */
+	tmp = STG_READ_REG(DACPixelFormat);
+	CLEAR_BITS_FRM_TO(0, 2);
+
+	/* Set LUT not used from 16bpp to 32 bpp ??? */
+	CLEAR_BITS_FRM_TO(8, 9);
+
+	switch (displayDepth) {
+	case 16:
+		{
+			physicalPixelDepth = 16;
+			tmp |= _16BPP;
+			break;
+		}
+	case 32:
+		{
+			/* Set for 32 bits per pixel */
+			physicalPixelDepth = 32;
+			tmp |= _32BPP;
+			break;
+		}
+	default:
+		return -EINVAL;
+	}
+
+	STG_WRITE_REG(DACPixelFormat, tmp);
+
+	/* Workout Bus transfer bandwidth according to pixel format */
+	ulPdiv = STG_PIXEL_BUS_WIDTH / physicalPixelDepth;
+
+	/* Get Screen Stride in pixels */
+	stride = displayWidth;
+
+	/* Set Primary size info */
+	tmp = STG_READ_REG(DACPrimSize);
+	CLEAR_BITS_FRM_TO(0, 10);
+	CLEAR_BITS_FRM_TO(12, 31);
+	tmp |=
+	    ((((displayHeight - 1) << 12) | (((displayWidth / ulPdiv) -
+					      1) << 23))
+	     | (stride / ulPdiv));
+	STG_WRITE_REG(DACPrimSize, tmp);
+
+
+	/* Set Pixel Clock */
+	*pixelClock = ProgramClock(REF_CLOCK, *pixelClock, &F, &R, &P);
+
+	/* Set DAC PLL Mode */
+	tmp = STG_READ_REG(DACPLLMode);
+	CLEAR_BITS_FRM_TO(0, 15);
+	/* tmp |= ((P-1) | ((F-2) << 2) | ((R-2) << 11)); */
+	tmp |= ((P) | ((F - 2) << 2) | ((R - 2) << 11));
+	STG_WRITE_REG(DACPLLMode, tmp);
+
+	/* Set Prim Address */
+	tmp = STG_READ_REG(DACPrimAddress);
+	CLEAR_BITS_FRM_TO(0, 20);
+	CLEAR_BITS_FRM_TO(20, 31);
+	STG_WRITE_REG(DACPrimAddress, tmp);
+
+	/* Set Cursor details with HW Cursor disabled */
+	tmp = STG_READ_REG(DACCursorCtrl);
+	tmp &= ~SET_BIT(31);
+	STG_WRITE_REG(DACCursorCtrl, tmp);
+
+	tmp = STG_READ_REG(DACCursorAddr);
+	CLEAR_BITS_FRM_TO(0, 20);
+	STG_WRITE_REG(DACCursorAddr, tmp);
+
+	/* Set Video Window */
+	tmp = STG_READ_REG(DACVidWinStart);
+	CLEAR_BITS_FRM_TO(0, 10);
+	CLEAR_BITS_FRM_TO(16, 26);
+	STG_WRITE_REG(DACVidWinStart, tmp);
+
+	tmp = STG_READ_REG(DACVidWinEnd);
+	CLEAR_BITS_FRM_TO(0, 10);
+	CLEAR_BITS_FRM_TO(16, 26);
+	STG_WRITE_REG(DACVidWinEnd, tmp);
+
+	/* Set DAC Border Color to default */
+	tmp = STG_READ_REG(DACBorderColor);
+	CLEAR_BITS_FRM_TO(0, 23);
+	STG_WRITE_REG(DACBorderColor, tmp);
+
+	/* Set Graphics and Overlay Burst Control */
+	STG_WRITE_REG(DACBurstCtrl, 0x0404);
+
+	/* Set CRC Trigger to default */
+	tmp = STG_READ_REG(DACCrcTrigger);
+	CLEAR_BIT(0);
+	STG_WRITE_REG(DACCrcTrigger, tmp);
+
+	/* Set Video Port Control to default */
+	tmp = STG_READ_REG(DigVidPortCtrl);
+	CLEAR_BIT(8);
+	CLEAR_BITS_FRM_TO(16, 27);
+	CLEAR_BITS_FRM_TO(1, 3);
+	CLEAR_BITS_FRM_TO(10, 11);
+	STG_WRITE_REG(DigVidPortCtrl, tmp);
+
+	return 0;
+}
+
+/* Ramdac control, turning output to the screen on and off */
+void DisableRamdacOutput(volatile STG4000REG __iomem * pSTGReg)
+{
+	u32 tmp;
+
+	/* Disable DAC for Graphics Stream Control */
+	tmp = (STG_READ_REG(DACStreamCtrl)) & ~SET_BIT(0);
+	STG_WRITE_REG(DACStreamCtrl, tmp);
+}
+
+void EnableRamdacOutput(volatile STG4000REG __iomem * pSTGReg)
+{
+	u32 tmp;
+
+	/* Enable DAC for Graphics Stream Control */
+	tmp = (STG_READ_REG(DACStreamCtrl)) | SET_BIT(0);
+	STG_WRITE_REG(DACStreamCtrl, tmp);
+}
diff --git a/kernel/drivers/video/fbdev/kyro/STG4000Reg.h b/kernel/drivers/video/fbdev/kyro/STG4000Reg.h
new file mode 100644
index 000000000..50f4670e9
--- /dev/null
+++ b/kernel/drivers/video/fbdev/kyro/STG4000Reg.h
@@ -0,0 +1,283 @@
+/*
+ *  linux/drivers/video/kyro/STG4000Reg.h
+ *
+ *  Copyright (C) 2002 STMicroelectronics
+ *
+ * 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.
+ */
+
+#ifndef _STG4000REG_H
+#define _STG4000REG_H
+
+#define DWFILL unsigned long :32
+#define WFILL unsigned short :16
+
+/*
+ * Macros that access memory mapped card registers in PCI space
+ * Add an appropriate section for your OS or processor architecture.
+ */
+#if defined(__KERNEL__)
+#include <asm/page.h>
+#include <asm/io.h>
+#define STG_WRITE_REG(reg,data) (writel(data,&pSTGReg->reg))
+#define STG_READ_REG(reg)      (readl(&pSTGReg->reg))
+#else
+#define STG_WRITE_REG(reg,data) (pSTGReg->reg = data)
+#define STG_READ_REG(reg)      (pSTGReg->reg)
+#endif /* __KERNEL__ */
+
+#define SET_BIT(n) (1<<(n))
+#define CLEAR_BIT(n) (tmp &= ~(1<<n))
+#define CLEAR_BITS_FRM_TO(frm, to) \
+{\
+int i; \
+    for(i = frm; i<= to; i++) \
+	{ \
+	    tmp &= ~(1<<i); \
+	} \
+}
+
+#define CLEAR_BIT_2(n) (usTemp &= ~(1<<n))
+#define CLEAR_BITS_FRM_TO_2(frm, to) \
+{\
+int i; \
+    for(i = frm; i<= to; i++) \
+	{ \
+	    usTemp &= ~(1<<i); \
+	} \
+}
+
+/* LUT select */
+typedef enum _LUT_USES {
+	NO_LUT = 0, RESERVED, GRAPHICS, OVERLAY
+} LUT_USES;
+
+/* Primary surface pixel format select */
+typedef enum _PIXEL_FORMAT {
+	_8BPP = 0, _15BPP, _16BPP, _24BPP, _32BPP
+} PIXEL_FORMAT;
+
+/* Overlay blending mode select */
+typedef enum _BLEND_MODE {
+	GRAPHICS_MODE = 0, COLOR_KEY, PER_PIXEL_ALPHA, GLOBAL_ALPHA,
+	CK_PIXEL_ALPHA, CK_GLOBAL_ALPHA
+} OVRL_BLEND_MODE;
+
+/* Overlay Pixel format select */
+typedef enum _OVRL_PIX_FORMAT {
+	UYVY, VYUY, YUYV, YVYU
+} OVRL_PIX_FORMAT;
+
+/* Register Table */
+typedef struct {
+	/* 0h  */
+	volatile u32 Thread0Enable;	/* 0x0000 */
+	volatile u32 Thread1Enable;	/* 0x0004 */
+	volatile u32 Thread0Recover;	/* 0x0008 */
+	volatile u32 Thread1Recover;	/* 0x000C */
+	volatile u32 Thread0Step;	/* 0x0010 */
+	volatile u32 Thread1Step;	/* 0x0014 */
+	volatile u32 VideoInStatus;	/* 0x0018 */
+	volatile u32 Core2InSignStart;	/* 0x001C */
+	volatile u32 Core1ResetVector;	/* 0x0020 */
+	volatile u32 Core1ROMOffset;	/* 0x0024 */
+	volatile u32 Core1ArbiterPriority;	/* 0x0028 */
+	volatile u32 VideoInControl;	/* 0x002C */
+	volatile u32 VideoInReg0CtrlA;	/* 0x0030 */
+	volatile u32 VideoInReg0CtrlB;	/* 0x0034 */
+	volatile u32 VideoInReg1CtrlA;	/* 0x0038 */
+	volatile u32 VideoInReg1CtrlB;	/* 0x003C */
+	volatile u32 Thread0Kicker;	/* 0x0040 */
+	volatile u32 Core2InputSign;	/* 0x0044 */
+	volatile u32 Thread0ProgCtr;	/* 0x0048 */
+	volatile u32 Thread1ProgCtr;	/* 0x004C */
+	volatile u32 Thread1Kicker;	/* 0x0050 */
+	volatile u32 GPRegister1;	/* 0x0054 */
+	volatile u32 GPRegister2;	/* 0x0058 */
+	volatile u32 GPRegister3;	/* 0x005C */
+	volatile u32 GPRegister4;	/* 0x0060 */
+	volatile u32 SerialIntA;	/* 0x0064 */
+
+	volatile u32 Fill0[6];	/* GAP 0x0068 - 0x007C */
+
+	volatile u32 SoftwareReset;	/* 0x0080 */
+	volatile u32 SerialIntB;	/* 0x0084 */
+
+	volatile u32 Fill1[37];	/* GAP 0x0088 - 0x011C */
+
+	volatile u32 ROMELQV;	/* 0x011C */
+	volatile u32 WLWH;	/* 0x0120 */
+	volatile u32 ROMELWL;	/* 0x0124 */
+
+	volatile u32 dwFill_1;	/* GAP 0x0128 */
+
+	volatile u32 IntStatus;	/* 0x012C */
+	volatile u32 IntMask;	/* 0x0130 */
+	volatile u32 IntClear;	/* 0x0134 */
+
+	volatile u32 Fill2[6];	/* GAP 0x0138 - 0x014C */
+
+	volatile u32 ROMGPIOA;	/* 0x0150 */
+	volatile u32 ROMGPIOB;	/* 0x0154 */
+	volatile u32 ROMGPIOC;	/* 0x0158 */
+	volatile u32 ROMGPIOD;	/* 0x015C */
+
+	volatile u32 Fill3[2];	/* GAP 0x0160 - 0x0168 */
+
+	volatile u32 AGPIntID;	/* 0x0168 */
+	volatile u32 AGPIntClassCode;	/* 0x016C */
+	volatile u32 AGPIntBIST;	/* 0x0170 */
+	volatile u32 AGPIntSSID;	/* 0x0174 */
+	volatile u32 AGPIntPMCSR;	/* 0x0178 */
+	volatile u32 VGAFrameBufBase;	/* 0x017C */
+	volatile u32 VGANotify;	/* 0x0180 */
+	volatile u32 DACPLLMode;	/* 0x0184 */
+	volatile u32 Core1VideoClockDiv;	/* 0x0188 */
+	volatile u32 AGPIntStat;	/* 0x018C */
+
+	/*
+	   volatile u32 Fill4[0x0400/4 - 0x0190/4]; //GAP 0x0190 - 0x0400
+	   volatile u32 Fill5[0x05FC/4 - 0x0400/4]; //GAP 0x0400 - 0x05FC Fog Table
+	   volatile u32 Fill6[0x0604/4 - 0x0600/4]; //GAP 0x0600 - 0x0604
+	   volatile u32 Fill7[0x0680/4 - 0x0608/4]; //GAP 0x0608 - 0x0680
+	   volatile u32 Fill8[0x07FC/4 - 0x0684/4]; //GAP 0x0684 - 0x07FC
+	 */
+	volatile u32 Fill4[412];	/* 0x0190 - 0x07FC */
+
+	volatile u32 TACtrlStreamBase;	/* 0x0800 */
+	volatile u32 TAObjDataBase;	/* 0x0804 */
+	volatile u32 TAPtrDataBase;	/* 0x0808 */
+	volatile u32 TARegionDataBase;	/* 0x080C */
+	volatile u32 TATailPtrBase;	/* 0x0810 */
+	volatile u32 TAPtrRegionSize;	/* 0x0814 */
+	volatile u32 TAConfiguration;	/* 0x0818 */
+	volatile u32 TAObjDataStartAddr;	/* 0x081C */
+	volatile u32 TAObjDataEndAddr;	/* 0x0820 */
+	volatile u32 TAXScreenClip;	/* 0x0824 */
+	volatile u32 TAYScreenClip;	/* 0x0828 */
+	volatile u32 TARHWClamp;	/* 0x082C */
+	volatile u32 TARHWCompare;	/* 0x0830 */
+	volatile u32 TAStart;	/* 0x0834 */
+	volatile u32 TAObjReStart;	/* 0x0838 */
+	volatile u32 TAPtrReStart;	/* 0x083C */
+	volatile u32 TAStatus1;	/* 0x0840 */
+	volatile u32 TAStatus2;	/* 0x0844 */
+	volatile u32 TAIntStatus;	/* 0x0848 */
+	volatile u32 TAIntMask;	/* 0x084C */
+
+	volatile u32 Fill5[235];	/* GAP 0x0850 - 0x0BF8 */
+
+	volatile u32 TextureAddrThresh;	/* 0x0BFC */
+	volatile u32 Core1Translation;	/* 0x0C00 */
+	volatile u32 TextureAddrReMap;	/* 0x0C04 */
+	volatile u32 RenderOutAGPRemap;	/* 0x0C08 */
+	volatile u32 _3DRegionReadTrans;	/* 0x0C0C */
+	volatile u32 _3DPtrReadTrans;	/* 0x0C10 */
+	volatile u32 _3DParamReadTrans;	/* 0x0C14 */
+	volatile u32 _3DRegionReadThresh;	/* 0x0C18 */
+	volatile u32 _3DPtrReadThresh;	/* 0x0C1C */
+	volatile u32 _3DParamReadThresh;	/* 0x0C20 */
+	volatile u32 _3DRegionReadAGPRemap;	/* 0x0C24 */
+	volatile u32 _3DPtrReadAGPRemap;	/* 0x0C28 */
+	volatile u32 _3DParamReadAGPRemap;	/* 0x0C2C */
+	volatile u32 ZBufferAGPRemap;	/* 0x0C30 */
+	volatile u32 TAIndexAGPRemap;	/* 0x0C34 */
+	volatile u32 TAVertexAGPRemap;	/* 0x0C38 */
+	volatile u32 TAUVAddrTrans;	/* 0x0C3C */
+	volatile u32 TATailPtrCacheTrans;	/* 0x0C40 */
+	volatile u32 TAParamWriteTrans;	/* 0x0C44 */
+	volatile u32 TAPtrWriteTrans;	/* 0x0C48 */
+	volatile u32 TAParamWriteThresh;	/* 0x0C4C */
+	volatile u32 TAPtrWriteThresh;	/* 0x0C50 */
+	volatile u32 TATailPtrCacheAGPRe;	/* 0x0C54 */
+	volatile u32 TAParamWriteAGPRe;	/* 0x0C58 */
+	volatile u32 TAPtrWriteAGPRe;	/* 0x0C5C */
+	volatile u32 SDRAMArbiterConf;	/* 0x0C60 */
+	volatile u32 SDRAMConf0;	/* 0x0C64 */
+	volatile u32 SDRAMConf1;	/* 0x0C68 */
+	volatile u32 SDRAMConf2;	/* 0x0C6C */
+	volatile u32 SDRAMRefresh;	/* 0x0C70 */
+	volatile u32 SDRAMPowerStat;	/* 0x0C74 */
+
+	volatile u32 Fill6[2];	/* GAP 0x0C78 - 0x0C7C */
+
+	volatile u32 RAMBistData;	/* 0x0C80 */
+	volatile u32 RAMBistCtrl;	/* 0x0C84 */
+	volatile u32 FIFOBistKey;	/* 0x0C88 */
+	volatile u32 RAMBistResult;	/* 0x0C8C */
+	volatile u32 FIFOBistResult;	/* 0x0C90 */
+
+	/*
+	   volatile u32 Fill11[0x0CBC/4 - 0x0C94/4]; //GAP 0x0C94 - 0x0CBC
+	   volatile u32 Fill12[0x0CD0/4 - 0x0CC0/4]; //GAP 0x0CC0 - 0x0CD0 3DRegisters
+	 */
+
+	volatile u32 Fill7[16];	/* 0x0c94 - 0x0cd0 */
+
+	volatile u32 SDRAMAddrSign;	/* 0x0CD4 */
+	volatile u32 SDRAMDataSign;	/* 0x0CD8 */
+	volatile u32 SDRAMSignConf;	/* 0x0CDC */
+
+	/* DWFILL; //GAP 0x0CE0 */
+	volatile u32 dwFill_2;
+
+	volatile u32 ISPSignature;	/* 0x0CE4 */
+
+	volatile u32 Fill8[454];	/*GAP 0x0CE8 - 0x13FC */
+
+	volatile u32 DACPrimAddress;	/* 0x1400 */
+	volatile u32 DACPrimSize;	/* 0x1404 */
+	volatile u32 DACCursorAddr;	/* 0x1408 */
+	volatile u32 DACCursorCtrl;	/* 0x140C */
+	volatile u32 DACOverlayAddr;	/* 0x1410 */
+	volatile u32 DACOverlayUAddr;	/* 0x1414 */
+	volatile u32 DACOverlayVAddr;	/* 0x1418 */
+	volatile u32 DACOverlaySize;	/* 0x141C */
+	volatile u32 DACOverlayVtDec;	/* 0x1420 */
+
+	volatile u32 Fill9[9];	/* GAP 0x1424 - 0x1444 */
+
+	volatile u32 DACVerticalScal;	/* 0x1448 */
+	volatile u32 DACPixelFormat;	/* 0x144C */
+	volatile u32 DACHorizontalScal;	/* 0x1450 */
+	volatile u32 DACVidWinStart;	/* 0x1454 */
+	volatile u32 DACVidWinEnd;	/* 0x1458 */
+	volatile u32 DACBlendCtrl;	/* 0x145C */
+	volatile u32 DACHorTim1;	/* 0x1460 */
+	volatile u32 DACHorTim2;	/* 0x1464 */
+	volatile u32 DACHorTim3;	/* 0x1468 */
+	volatile u32 DACVerTim1;	/* 0x146C */
+	volatile u32 DACVerTim2;	/* 0x1470 */
+	volatile u32 DACVerTim3;	/* 0x1474 */
+	volatile u32 DACBorderColor;	/* 0x1478 */
+	volatile u32 DACSyncCtrl;	/* 0x147C */
+	volatile u32 DACStreamCtrl;	/* 0x1480 */
+	volatile u32 DACLUTAddress;	/* 0x1484 */
+	volatile u32 DACLUTData;	/* 0x1488 */
+	volatile u32 DACBurstCtrl;	/* 0x148C */
+	volatile u32 DACCrcTrigger;	/* 0x1490 */
+	volatile u32 DACCrcDone;	/* 0x1494 */
+	volatile u32 DACCrcResult1;	/* 0x1498 */
+	volatile u32 DACCrcResult2;	/* 0x149C */
+	volatile u32 DACLinecount;	/* 0x14A0 */
+
+	volatile u32 Fill10[151];	/*GAP 0x14A4 - 0x16FC */
+
+	volatile u32 DigVidPortCtrl;	/* 0x1700 */
+	volatile u32 DigVidPortStat;	/* 0x1704 */
+
+	/*
+	   volatile u32 Fill11[0x1FFC/4 - 0x1708/4]; //GAP 0x1708 - 0x1FFC
+	   volatile u32 Fill17[0x3000/4 - 0x2FFC/4]; //GAP 0x2000 - 0x2FFC ALUT
+	 */
+
+	volatile u32 Fill11[1598];
+
+	/* DWFILL; //GAP 0x3000          ALUT 256MB offset */
+	volatile u32 Fill_3;
+
+} STG4000REG;
+
+#endif /* _STG4000REG_H */
diff --git a/kernel/drivers/video/fbdev/kyro/STG4000VTG.c b/kernel/drivers/video/fbdev/kyro/STG4000VTG.c
new file mode 100644
index 000000000..bd389709d
--- /dev/null
+++ b/kernel/drivers/video/fbdev/kyro/STG4000VTG.c
@@ -0,0 +1,170 @@
+/*
+ *  linux/drivers/video/kyro/STG4000VTG.c
+ *
+ *  Copyright (C) 2002 STMicroelectronics
+ *
+ * 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 <linux/types.h>
+#include <video/kyro.h>
+
+#include "STG4000Reg.h"
+#include "STG4000Interface.h"
+
+void DisableVGA(volatile STG4000REG __iomem *pSTGReg)
+{
+	u32 tmp;
+	volatile u32 count = 0, i;
+
+	/* Reset the VGA registers */
+	tmp = STG_READ_REG(SoftwareReset);
+	CLEAR_BIT(8);
+	STG_WRITE_REG(SoftwareReset, tmp);
+
+	/* Just for Delay */
+	for (i = 0; i < 1000; i++) {
+		count++;
+	}
+
+	/* Pull-out the VGA registers from reset */
+	tmp = STG_READ_REG(SoftwareReset);
+	tmp |= SET_BIT(8);
+	STG_WRITE_REG(SoftwareReset, tmp);
+}
+
+void StopVTG(volatile STG4000REG __iomem *pSTGReg)
+{
+	u32 tmp = 0;
+
+	/* Stop Ver and Hor Sync Generator */
+	tmp = (STG_READ_REG(DACSyncCtrl)) | SET_BIT(0) | SET_BIT(2);
+	CLEAR_BIT(31);
+	STG_WRITE_REG(DACSyncCtrl, tmp);
+}
+
+void StartVTG(volatile STG4000REG __iomem *pSTGReg)
+{
+	u32 tmp = 0;
+
+	/* Start Ver and Hor Sync Generator */
+	tmp = ((STG_READ_REG(DACSyncCtrl)) | SET_BIT(31));
+	CLEAR_BIT(0);
+	CLEAR_BIT(2);
+	STG_WRITE_REG(DACSyncCtrl, tmp);
+}
+
+void SetupVTG(volatile STG4000REG __iomem *pSTGReg,
+	      const struct kyrofb_info * pTiming)
+{
+	u32 tmp = 0;
+	u32 margins = 0;
+	u32 ulBorder;
+	u32 xRes = pTiming->XRES;
+	u32 yRes = pTiming->YRES;
+
+	/* Horizontal */
+	u32 HAddrTime, HRightBorder, HLeftBorder;
+	u32 HBackPorcStrt, HFrontPorchStrt, HTotal,
+	    HLeftBorderStrt, HRightBorderStrt, HDisplayStrt;
+
+	/* Vertical */
+	u32 VDisplayStrt, VBottomBorder, VTopBorder;
+	u32 VBackPorchStrt, VTotal, VTopBorderStrt,
+	    VFrontPorchStrt, VBottomBorderStrt, VAddrTime;
+
+	/* Need to calculate the right border */
+	if ((xRes == 640) && (yRes == 480)) {
+		if ((pTiming->VFREQ == 60) || (pTiming->VFREQ == 72)) {
+			margins = 8;
+		}
+	}
+
+	/* Work out the Border */
+	ulBorder =
+	    (pTiming->HTot -
+	     (pTiming->HST + (pTiming->HBP - margins) + xRes +
+	      (pTiming->HFP - margins))) >> 1;
+
+	/* Border the same for Vertical and Horizontal */
+	VBottomBorder = HLeftBorder = VTopBorder = HRightBorder = ulBorder;
+
+    /************ Get Timing values for Horizontal ******************/
+	HAddrTime = xRes;
+	HBackPorcStrt = pTiming->HST;
+	HTotal = pTiming->HTot;
+	HDisplayStrt =
+	    pTiming->HST + (pTiming->HBP - margins) + HLeftBorder;
+	HLeftBorderStrt = HDisplayStrt - HLeftBorder;
+	HFrontPorchStrt =
+	    pTiming->HST + (pTiming->HBP - margins) + HLeftBorder +
+	    HAddrTime + HRightBorder;
+	HRightBorderStrt = HFrontPorchStrt - HRightBorder;
+
+    /************ Get Timing values for Vertical ******************/
+	VAddrTime = yRes;
+	VBackPorchStrt = pTiming->VST;
+	VTotal = pTiming->VTot;
+	VDisplayStrt =
+	    pTiming->VST + (pTiming->VBP - margins) + VTopBorder;
+	VTopBorderStrt = VDisplayStrt - VTopBorder;
+	VFrontPorchStrt =
+	    pTiming->VST + (pTiming->VBP - margins) + VTopBorder +
+	    VAddrTime + VBottomBorder;
+	VBottomBorderStrt = VFrontPorchStrt - VBottomBorder;
+
+	/* Set Hor Timing 1, 2, 3 */
+	tmp = STG_READ_REG(DACHorTim1);
+	CLEAR_BITS_FRM_TO(0, 11);
+	CLEAR_BITS_FRM_TO(16, 27);
+	tmp |= (HTotal) | (HBackPorcStrt << 16);
+	STG_WRITE_REG(DACHorTim1, tmp);
+
+	tmp = STG_READ_REG(DACHorTim2);
+	CLEAR_BITS_FRM_TO(0, 11);
+	CLEAR_BITS_FRM_TO(16, 27);
+	tmp |= (HDisplayStrt << 16) | HLeftBorderStrt;
+	STG_WRITE_REG(DACHorTim2, tmp);
+
+	tmp = STG_READ_REG(DACHorTim3);
+	CLEAR_BITS_FRM_TO(0, 11);
+	CLEAR_BITS_FRM_TO(16, 27);
+	tmp |= (HFrontPorchStrt << 16) | HRightBorderStrt;
+	STG_WRITE_REG(DACHorTim3, tmp);
+
+	/* Set Ver Timing 1, 2, 3 */
+	tmp = STG_READ_REG(DACVerTim1);
+	CLEAR_BITS_FRM_TO(0, 11);
+	CLEAR_BITS_FRM_TO(16, 27);
+	tmp |= (VBackPorchStrt << 16) | (VTotal);
+	STG_WRITE_REG(DACVerTim1, tmp);
+
+	tmp = STG_READ_REG(DACVerTim2);
+	CLEAR_BITS_FRM_TO(0, 11);
+	CLEAR_BITS_FRM_TO(16, 27);
+	tmp |= (VDisplayStrt << 16) | VTopBorderStrt;
+	STG_WRITE_REG(DACVerTim2, tmp);
+
+	tmp = STG_READ_REG(DACVerTim3);
+	CLEAR_BITS_FRM_TO(0, 11);
+	CLEAR_BITS_FRM_TO(16, 27);
+	tmp |= (VFrontPorchStrt << 16) | VBottomBorderStrt;
+	STG_WRITE_REG(DACVerTim3, tmp);
+
+	/* Set Verical and Horizontal Polarity */
+	tmp = STG_READ_REG(DACSyncCtrl) | SET_BIT(3) | SET_BIT(1);
+
+	if ((pTiming->HSP > 0) && (pTiming->VSP < 0)) {	/* +hsync -vsync */
+		tmp &= ~0x8;
+	} else if ((pTiming->HSP < 0) && (pTiming->VSP > 0)) {	/* -hsync +vsync */
+		tmp &= ~0x2;
+	} else if ((pTiming->HSP < 0) && (pTiming->VSP < 0)) {	/* -hsync -vsync */
+		tmp &= ~0xA;
+	} else if ((pTiming->HSP > 0) && (pTiming->VSP > 0)) {	/* +hsync -vsync */
+		tmp &= ~0x0;
+	}
+
+	STG_WRITE_REG(DACSyncCtrl, tmp);
+}
diff --git a/kernel/drivers/video/fbdev/kyro/fbdev.c b/kernel/drivers/video/fbdev/kyro/fbdev.c
new file mode 100644
index 000000000..65041e15f
--- /dev/null
+++ b/kernel/drivers/video/fbdev/kyro/fbdev.c
@@ -0,0 +1,808 @@
+/*
+ *  linux/drivers/video/kyro/fbdev.c
+ *
+ *  Copyright (C) 2002 STMicroelectronics
+ *  Copyright (C) 2003, 2004 Paul Mundt
+ *
+ * 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 <linux/module.h>
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/errno.h>
+#include <linux/string.h>
+#include <linux/delay.h>
+#include <linux/fb.h>
+#include <linux/ioctl.h>
+#include <linux/init.h>
+#include <linux/pci.h>
+#include <asm/io.h>
+#include <linux/uaccess.h>
+#ifdef CONFIG_MTRR
+#include <asm/mtrr.h>
+#endif
+
+#include <video/kyro.h>
+
+#include "STG4000Reg.h"
+#include "STG4000Interface.h"
+
+/*
+ * PCI Definitions
+ */
+#define PCI_VENDOR_ID_ST	0x104a
+#define PCI_DEVICE_ID_STG4000	0x0010
+
+#define KHZ2PICOS(a) (1000000000UL/(a))
+
+/****************************************************************************/
+static struct fb_fix_screeninfo kyro_fix = {
+	.id		= "ST Kyro",
+	.type		= FB_TYPE_PACKED_PIXELS,
+	.visual		= FB_VISUAL_TRUECOLOR,
+	.accel		= FB_ACCEL_NONE,
+};
+
+static struct fb_var_screeninfo kyro_var = {
+	/* 640x480, 16bpp @ 60 Hz */
+	.xres		= 640,
+	.yres		= 480,
+	.xres_virtual	= 640,
+	.yres_virtual	= 480,
+	.bits_per_pixel	= 16,
+	.red		= { 11, 5, 0 },
+	.green		= {  5, 6, 0 },
+	.blue		= {  0, 5, 0 },
+	.activate	= FB_ACTIVATE_NOW,
+	.height		= -1,
+	.width		= -1,
+	.pixclock	= KHZ2PICOS(25175),
+	.left_margin	= 48,
+	.right_margin	= 16,
+	.upper_margin	= 33,
+	.lower_margin	= 10,
+	.hsync_len	= 96,
+	.vsync_len	= 2,
+	.vmode		= FB_VMODE_NONINTERLACED,
+};
+
+typedef struct {
+	STG4000REG __iomem *pSTGReg;	/* Virtual address of PCI register region */
+	u32 ulNextFreeVidMem;	/* Offset from start of vid mem to next free region */
+	u32 ulOverlayOffset;	/* Offset from start of vid mem to overlay */
+	u32 ulOverlayStride;	/* Interleaved YUV and 422 mode Y stride */
+	u32 ulOverlayUVStride;	/* 422 mode U & V stride */
+} device_info_t;
+
+/* global graphics card info structure (one per card) */
+static device_info_t deviceInfo;
+
+static char *mode_option = NULL;
+static int nopan = 0;
+static int nowrap = 1;
+#ifdef CONFIG_MTRR
+static int nomtrr = 0;
+#endif
+
+/* PCI driver prototypes */
+static int kyrofb_probe(struct pci_dev *pdev, const struct pci_device_id *ent);
+static void kyrofb_remove(struct pci_dev *pdev);
+
+static struct fb_videomode kyro_modedb[] = {
+	{
+		/* 640x350 @ 85Hz */
+		NULL, 85, 640, 350, KHZ2PICOS(31500),
+		96, 32, 60, 32, 64, 3,
+		FB_SYNC_HOR_HIGH_ACT, FB_VMODE_NONINTERLACED
+	}, {
+		/* 640x400 @ 85Hz */
+		NULL, 85, 640, 400, KHZ2PICOS(31500),
+		96, 32, 41, 1, 64, 3,
+		FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
+	}, {
+		/* 720x400 @ 85Hz */
+		NULL, 85, 720, 400, KHZ2PICOS(35500),
+		108, 36, 42, 1, 72, 3,
+		FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
+	}, {
+		/* 640x480 @ 60Hz */
+		NULL, 60, 640, 480, KHZ2PICOS(25175),
+		48, 16, 33, 10, 96, 2,
+		0, FB_VMODE_NONINTERLACED
+	}, {
+		/* 640x480 @ 72Hz */
+		NULL, 72, 640, 480, KHZ2PICOS(31500),
+		128, 24, 28, 9, 40, 3,
+		0, FB_VMODE_NONINTERLACED
+	}, {
+		/* 640x480 @ 75Hz */
+		NULL, 75, 640, 480, KHZ2PICOS(31500),
+		120, 16, 16, 1, 64, 3,
+		0, FB_VMODE_NONINTERLACED
+	}, {
+		/* 640x480 @ 85Hz */
+		NULL, 85, 640, 480, KHZ2PICOS(36000),
+		80, 56, 25, 1, 56, 3,
+		0, FB_VMODE_NONINTERLACED
+	}, {
+		/* 800x600 @ 56Hz */
+		NULL, 56, 800, 600, KHZ2PICOS(36000),
+		128, 24, 22, 1, 72, 2,
+		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
+	}, {
+		/* 800x600 @ 60Hz */
+		NULL, 60, 800, 600, KHZ2PICOS(40000),
+		88, 40, 23, 1, 128, 4,
+		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
+	}, {
+		/* 800x600 @ 72Hz */
+		NULL, 72, 800, 600, KHZ2PICOS(50000),
+		64, 56, 23, 37, 120, 6,
+		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
+	}, {
+		/* 800x600 @ 75Hz */
+		NULL, 75, 800, 600, KHZ2PICOS(49500),
+		160, 16, 21, 1, 80, 3,
+		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
+	}, {
+		/* 800x600 @ 85Hz */
+		NULL, 85, 800, 600, KHZ2PICOS(56250),
+		152, 32, 27, 1, 64, 3,
+		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1024x768 @ 60Hz */
+		NULL, 60, 1024, 768, KHZ2PICOS(65000),
+		160, 24, 29, 3, 136, 6,
+		0, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1024x768 @ 70Hz */
+		NULL, 70, 1024, 768, KHZ2PICOS(75000),
+		144, 24, 29, 3, 136, 6,
+		0, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1024x768 @ 75Hz */
+		NULL, 75, 1024, 768, KHZ2PICOS(78750),
+		176, 16, 28, 1, 96, 3,
+		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1024x768 @ 85Hz */
+		NULL, 85, 1024, 768, KHZ2PICOS(94500),
+		208, 48, 36, 1, 96, 3,
+		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1152x864 @ 75Hz */
+		NULL, 75, 1152, 864, KHZ2PICOS(108000),
+		256, 64, 32, 1, 128, 3,
+		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1280x960 @ 60Hz */
+		NULL, 60, 1280, 960, KHZ2PICOS(108000),
+		312, 96, 36, 1, 112, 3,
+		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1280x960 @ 85Hz */
+		NULL, 85, 1280, 960, KHZ2PICOS(148500),
+		224, 64, 47, 1, 160, 3,
+		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1280x1024 @ 60Hz */
+		NULL, 60, 1280, 1024, KHZ2PICOS(108000),
+		248, 48, 38, 1, 112, 3,
+		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1280x1024 @ 75Hz */
+		NULL, 75, 1280, 1024, KHZ2PICOS(135000),
+		248, 16, 38, 1, 144, 3,
+		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1280x1024 @ 85Hz */
+		NULL, 85, 1280, 1024, KHZ2PICOS(157500),
+		224, 64, 44, 1, 160, 3,
+		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1600x1200 @ 60Hz */
+		NULL, 60, 1600, 1200, KHZ2PICOS(162000),
+		304, 64, 46, 1, 192, 3,
+		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1600x1200 @ 65Hz */
+		NULL, 65, 1600, 1200, KHZ2PICOS(175500),
+		304, 64, 46, 1, 192, 3,
+		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1600x1200 @ 70Hz */
+		NULL, 70, 1600, 1200, KHZ2PICOS(189000),
+		304, 64, 46, 1, 192, 3,
+		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1600x1200 @ 75Hz */
+		NULL, 75, 1600, 1200, KHZ2PICOS(202500),
+		304, 64, 46, 1, 192, 3,
+		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1600x1200 @ 85Hz */
+		NULL, 85, 1600, 1200, KHZ2PICOS(229500),
+		304, 64, 46, 1, 192, 3,
+		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1792x1344 @ 60Hz */
+		NULL, 60, 1792, 1344, KHZ2PICOS(204750),
+		328, 128, 46, 1, 200, 3,
+		FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1792x1344 @ 75Hz */
+		NULL, 75, 1792, 1344, KHZ2PICOS(261000),
+		352, 96, 69, 1, 216, 3,
+		FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1856x1392 @ 60Hz */
+		NULL, 60, 1856, 1392, KHZ2PICOS(218250),
+		352, 96, 43, 1, 224, 3,
+		FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1856x1392 @ 75Hz */
+		NULL, 75, 1856, 1392, KHZ2PICOS(288000),
+		352, 128, 104, 1, 224, 3,
+		FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1920x1440 @ 60Hz */
+		NULL, 60, 1920, 1440, KHZ2PICOS(234000),
+		344, 128, 56, 1, 208, 3,
+		FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
+	}, {
+		/* 1920x1440 @ 75Hz */
+		NULL, 75, 1920, 1440, KHZ2PICOS(297000),
+		352, 144, 56, 1, 224, 3,
+		FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
+	},
+};
+#define NUM_TOTAL_MODES	ARRAY_SIZE(kyro_modedb)
+
+/*
+ * This needs to be kept ordered corresponding to kyro_modedb.
+ */
+enum {
+	VMODE_640_350_85,
+	VMODE_640_400_85,
+	VMODE_720_400_85,
+	VMODE_640_480_60,
+	VMODE_640_480_72,
+	VMODE_640_480_75,
+	VMODE_640_480_85,
+	VMODE_800_600_56,
+	VMODE_800_600_60,
+	VMODE_800_600_72,
+	VMODE_800_600_75,
+	VMODE_800_600_85,
+	VMODE_1024_768_60,
+	VMODE_1024_768_70,
+	VMODE_1024_768_75,
+	VMODE_1024_768_85,
+	VMODE_1152_864_75,
+	VMODE_1280_960_60,
+	VMODE_1280_960_85,
+	VMODE_1280_1024_60,
+	VMODE_1280_1024_75,
+	VMODE_1280_1024_85,
+	VMODE_1600_1200_60,
+	VMODE_1600_1200_65,
+	VMODE_1600_1200_70,
+	VMODE_1600_1200_75,
+	VMODE_1600_1200_85,
+	VMODE_1792_1344_60,
+	VMODE_1792_1344_75,
+	VMODE_1856_1392_60,
+	VMODE_1856_1392_75,
+	VMODE_1920_1440_60,
+	VMODE_1920_1440_75,
+};
+
+/* Accessors */
+static int kyro_dev_video_mode_set(struct fb_info *info)
+{
+	struct kyrofb_info *par = info->par;
+
+	/* Turn off display */
+	StopVTG(deviceInfo.pSTGReg);
+	DisableRamdacOutput(deviceInfo.pSTGReg);
+
+	/* Bring us out of VGA and into Hi-Res mode, if not already. */
+	DisableVGA(deviceInfo.pSTGReg);
+
+	if (InitialiseRamdac(deviceInfo.pSTGReg,
+			     info->var.bits_per_pixel,
+			     info->var.xres, info->var.yres,
+			     par->HSP, par->VSP, &par->PIXCLK) < 0)
+		return -EINVAL;
+
+	SetupVTG(deviceInfo.pSTGReg, par);
+
+	ResetOverlayRegisters(deviceInfo.pSTGReg);
+
+	/* Turn on display in new mode */
+	EnableRamdacOutput(deviceInfo.pSTGReg);
+	StartVTG(deviceInfo.pSTGReg);
+
+	deviceInfo.ulNextFreeVidMem = info->var.xres * info->var.yres *
+				      info->var.bits_per_pixel;
+	deviceInfo.ulOverlayOffset = 0;
+
+	return 0;
+}
+
+static int kyro_dev_overlay_create(u32 ulWidth,
+				   u32 ulHeight, int bLinear)
+{
+	u32 offset;
+	u32 stride, uvStride;
+
+	if (deviceInfo.ulOverlayOffset != 0)
+		/*
+		 * Can only create one overlay without resetting the card or
+		 * changing display mode
+		 */
+		return -EINVAL;
+
+	ResetOverlayRegisters(deviceInfo.pSTGReg);
+
+	/* Overlays are addressed in multiples of 16bytes or 32bytes, so make
+	 * sure the start offset is on an appropriate boundary.
+	 */
+	offset = deviceInfo.ulNextFreeVidMem;
+	if ((offset & 0x1f) != 0) {
+		offset = (offset + 32L) & 0xffffffE0L;
+	}
+
+	if (CreateOverlaySurface(deviceInfo.pSTGReg, ulWidth, ulHeight,
+				 bLinear, offset, &stride, &uvStride) < 0)
+		return -EINVAL;
+
+	deviceInfo.ulOverlayOffset = offset;
+	deviceInfo.ulOverlayStride = stride;
+	deviceInfo.ulOverlayUVStride = uvStride;
+	deviceInfo.ulNextFreeVidMem = offset + (ulHeight * stride) + (ulHeight * 2 * uvStride);
+
+	SetOverlayBlendMode(deviceInfo.pSTGReg, GLOBAL_ALPHA, 0xf, 0x0);
+
+	return 0;
+}
+
+static int kyro_dev_overlay_viewport_set(u32 x, u32 y, u32 ulWidth, u32 ulHeight)
+{
+	if (deviceInfo.ulOverlayOffset == 0)
+		/* probably haven't called CreateOverlay yet */
+		return -EINVAL;
+
+	/* Stop Ramdac Output */
+	DisableRamdacOutput(deviceInfo.pSTGReg);
+
+	SetOverlayViewPort(deviceInfo.pSTGReg,
+			   x, y, x + ulWidth - 1, y + ulHeight - 1);
+
+	EnableOverlayPlane(deviceInfo.pSTGReg);
+	/* Start Ramdac Output */
+	EnableRamdacOutput(deviceInfo.pSTGReg);
+
+	return 0;
+}
+
+static inline unsigned long get_line_length(int x, int bpp)
+{
+	return (unsigned long)((((x*bpp)+31)&~31) >> 3);
+}
+
+static int kyrofb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
+{
+	struct kyrofb_info *par = info->par;
+
+	if (var->bits_per_pixel != 16 && var->bits_per_pixel != 32) {
+		printk(KERN_WARNING "kyrofb: depth not supported: %u\n", var->bits_per_pixel);
+		return -EINVAL;
+	}
+
+	switch (var->bits_per_pixel) {
+	case 16:
+		var->red.offset = 11;
+		var->red.length = 5;
+		var->green.offset = 5;
+		var->green.length = 6;
+		var->blue.length = 5;
+		break;
+	case 32:
+		var->transp.offset = 24;
+		var->red.offset = 16;
+		var->green.offset = 8;
+		var->blue.offset = 0;
+
+		var->red.length = 8;
+		var->green.length = 8;
+		var->blue.length = 8;
+		var->transp.length = 8;
+		break;
+	}
+
+	/* Height/Width of picture in mm */
+	var->height = var->width = -1;
+
+	/* Timing information. All values are in picoseconds */
+
+	/* par->PIXCLK is in 100Hz units. Convert to picoseconds -
+	 * ensuring we do not exceed 32 bit precision
+	 */
+	/*
+	 * XXX: Enabling this really screws over the pixclock value when we
+	 * read it back with fbset. As such, leaving this commented out appears
+	 * to do the right thing (at least for now) .. bearing in mind that we
+	 * have infact already done the KHZ2PICOS conversion in both the modedb
+	 * and kyro_var. -- PFM.
+	 */
+//	var->pixclock = 1000000000 / (par->PIXCLK / 10);
+
+	/* the header file claims we should use picoseconds
+	 * - nobody else does though, the all use pixels and lines
+	 * of h and v sizes. Both options here.
+	 */
+
+	/*
+	 * If we're being called by __fb_try_mode(), then we don't want to
+	 * override any of the var settings that we've already parsed
+	 * from our modedb. -- PFM.
+	 */
+	if ((var->activate & FB_ACTIVATE_MASK) == FB_ACTIVATE_TEST)
+		return 0;
+
+	var->left_margin = par->HBP;
+	var->hsync_len = par->HST;
+	var->right_margin = par->HFP;
+
+	var->upper_margin = par->VBP;
+	var->vsync_len = par->VST;
+	var->lower_margin = par->VFP;
+
+	if (par->HSP == 1)
+		var->sync |= FB_SYNC_HOR_HIGH_ACT;
+	if (par->VSP == 1)
+		var->sync |= FB_SYNC_VERT_HIGH_ACT;
+
+	return 0;
+}
+
+static int kyrofb_set_par(struct fb_info *info)
+{
+	struct kyrofb_info *par = info->par;
+	unsigned long lineclock;
+	unsigned long frameclock;
+
+	/* Actual resolution */
+	par->XRES = info->var.xres;
+	par->YRES = info->var.yres;
+
+	/* pixel depth */
+	par->PIXDEPTH = info->var.bits_per_pixel;
+
+	/* Refresh rate */
+	/* time for a line in ns */
+	lineclock = (info->var.pixclock * (info->var.xres +
+				    info->var.right_margin +
+				    info->var.hsync_len +
+				    info->var.left_margin)) / 1000;
+
+
+	/* time for a frame in ns (precision in 32bpp) */
+	frameclock = lineclock * (info->var.yres +
+				  info->var.lower_margin +
+				  info->var.vsync_len +
+				  info->var.upper_margin);
+
+	/* Calculate refresh rate and horrizontal clocks */
+	par->VFREQ = (1000000000 + (frameclock / 2)) / frameclock;
+	par->HCLK = (1000000000 + (lineclock / 2)) / lineclock;
+	par->PIXCLK = ((1000000000 + (info->var.pixclock / 2))
+					/ info->var.pixclock) * 10;
+
+	/* calculate horizontal timings */
+	par->HFP = info->var.right_margin;
+	par->HST = info->var.hsync_len;
+	par->HBP = info->var.left_margin;
+	par->HTot = par->XRES + par->HBP + par->HST + par->HFP;
+
+	/* calculate vertical timings */
+	par->VFP = info->var.lower_margin;
+	par->VST = info->var.vsync_len;
+	par->VBP = info->var.upper_margin;
+	par->VTot = par->YRES + par->VBP + par->VST + par->VFP;
+
+	par->HSP = (info->var.sync & FB_SYNC_HOR_HIGH_ACT) ? 1 : 0;
+	par->VSP = (info->var.sync & FB_SYNC_VERT_HIGH_ACT) ? 1 : 0;
+
+	kyro_dev_video_mode_set(info);
+
+	/* length of a line in bytes    */
+	info->fix.line_length = get_line_length(par->XRES, par->PIXDEPTH);
+	info->fix.visual = FB_VISUAL_TRUECOLOR;
+
+	return 0;
+}
+
+static int kyrofb_setcolreg(u_int regno, u_int red, u_int green,
+			    u_int blue, u_int transp, struct fb_info *info)
+{
+	struct kyrofb_info *par = info->par;
+
+	if (regno > 255)
+		return 1;	/* Invalid register */
+
+	if (regno < 16) {
+		switch (info->var.bits_per_pixel) {
+		case 16:
+			par->palette[regno] =
+			     (red   & 0xf800) |
+			    ((green & 0xfc00) >> 5) |
+			    ((blue  & 0xf800) >> 11);
+			break;
+		case 32:
+			red >>= 8; green >>= 8; blue >>= 8; transp >>= 8;
+			par->palette[regno] =
+			    (transp << 24) | (red << 16) | (green << 8) | blue;
+			break;
+		}
+	}
+
+	return 0;
+}
+
+#ifndef MODULE
+static int __init kyrofb_setup(char *options)
+{
+	char *this_opt;
+
+	if (!options || !*options)
+		return 0;
+
+	while ((this_opt = strsep(&options, ","))) {
+		if (!*this_opt)
+			continue;
+		if (strcmp(this_opt, "nopan") == 0) {
+			nopan = 1;
+		} else if (strcmp(this_opt, "nowrap") == 0) {
+			nowrap = 1;
+#ifdef CONFIG_MTRR
+		} else if (strcmp(this_opt, "nomtrr") == 0) {
+			nomtrr = 1;
+#endif
+		} else {
+			mode_option = this_opt;
+		}
+	}
+
+	return 0;
+}
+#endif
+
+static int kyrofb_ioctl(struct fb_info *info,
+			unsigned int cmd, unsigned long arg)
+{
+	overlay_create ol_create;
+	overlay_viewport_set ol_viewport_set;
+	void __user *argp = (void __user *)arg;
+
+	switch (cmd) {
+	case KYRO_IOCTL_OVERLAY_CREATE:
+		if (copy_from_user(&ol_create, argp, sizeof(overlay_create)))
+			return -EFAULT;
+
+		if (kyro_dev_overlay_create(ol_create.ulWidth,
+					    ol_create.ulHeight, 0) < 0) {
+			printk(KERN_ERR "Kyro FB: failed to create overlay surface.\n");
+
+			return -EINVAL;
+		}
+		break;
+	case KYRO_IOCTL_OVERLAY_VIEWPORT_SET:
+		if (copy_from_user(&ol_viewport_set, argp,
+			       sizeof(overlay_viewport_set)))
+			return -EFAULT;
+
+		if (kyro_dev_overlay_viewport_set(ol_viewport_set.xOrgin,
+						  ol_viewport_set.yOrgin,
+						  ol_viewport_set.xSize,
+						  ol_viewport_set.ySize) != 0)
+		{
+			printk(KERN_ERR "Kyro FB: failed to create overlay viewport.\n");
+			return -EINVAL;
+		}
+		break;
+	case KYRO_IOCTL_SET_VIDEO_MODE:
+		{
+			printk(KERN_ERR "Kyro FB: KYRO_IOCTL_SET_VIDEO_MODE is"
+				"obsolete, use the appropriate fb_ioctl()"
+				"command instead.\n");
+			return -EINVAL;
+		}
+	case KYRO_IOCTL_UVSTRIDE:
+		if (copy_to_user(argp, &deviceInfo.ulOverlayUVStride, sizeof(deviceInfo.ulOverlayUVStride)))
+			return -EFAULT;
+		break;
+	case KYRO_IOCTL_STRIDE:
+		if (copy_to_user(argp, &deviceInfo.ulOverlayStride, sizeof(deviceInfo.ulOverlayStride)))
+			return -EFAULT;
+		break;
+	case KYRO_IOCTL_OVERLAY_OFFSET:
+		if (copy_to_user(argp, &deviceInfo.ulOverlayOffset, sizeof(deviceInfo.ulOverlayOffset)))
+			return -EFAULT;
+		break;
+	}
+
+	return 0;
+}
+
+static struct pci_device_id kyrofb_pci_tbl[] = {
+	{ PCI_VENDOR_ID_ST, PCI_DEVICE_ID_STG4000,
+	  PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0 },
+	{ 0, }
+};
+
+MODULE_DEVICE_TABLE(pci, kyrofb_pci_tbl);
+
+static struct pci_driver kyrofb_pci_driver = {
+	.name		= "kyrofb",
+	.id_table	= kyrofb_pci_tbl,
+	.probe		= kyrofb_probe,
+	.remove		= kyrofb_remove,
+};
+
+static struct fb_ops kyrofb_ops = {
+	.owner		= THIS_MODULE,
+	.fb_check_var	= kyrofb_check_var,
+	.fb_set_par	= kyrofb_set_par,
+	.fb_setcolreg	= kyrofb_setcolreg,
+	.fb_ioctl	= kyrofb_ioctl,
+	.fb_fillrect	= cfb_fillrect,
+	.fb_copyarea	= cfb_copyarea,
+	.fb_imageblit	= cfb_imageblit,
+};
+
+static int kyrofb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
+{
+	struct fb_info *info;
+	struct kyrofb_info *currentpar;
+	unsigned long size;
+	int err;
+
+	if ((err = pci_enable_device(pdev))) {
+		printk(KERN_WARNING "kyrofb: Can't enable pdev: %d\n", err);
+		return err;
+	}
+
+	info = framebuffer_alloc(sizeof(struct kyrofb_info), &pdev->dev);
+	if (!info)
+		return -ENOMEM;
+
+	currentpar = info->par;
+
+	kyro_fix.smem_start = pci_resource_start(pdev, 0);
+	kyro_fix.smem_len   = pci_resource_len(pdev, 0);
+	kyro_fix.mmio_start = pci_resource_start(pdev, 1);
+	kyro_fix.mmio_len   = pci_resource_len(pdev, 1);
+
+	currentpar->regbase = deviceInfo.pSTGReg =
+		ioremap_nocache(kyro_fix.mmio_start, kyro_fix.mmio_len);
+
+	info->screen_base = ioremap_nocache(kyro_fix.smem_start,
+					    kyro_fix.smem_len);
+
+#ifdef CONFIG_MTRR
+	if (!nomtrr)
+		currentpar->mtrr_handle =
+			mtrr_add(kyro_fix.smem_start,
+				 kyro_fix.smem_len,
+				 MTRR_TYPE_WRCOMB, 1);
+#endif
+
+	kyro_fix.ypanstep	= nopan ? 0 : 1;
+	kyro_fix.ywrapstep	= nowrap ? 0 : 1;
+
+	info->fbops		= &kyrofb_ops;
+	info->fix		= kyro_fix;
+	info->pseudo_palette	= currentpar->palette;
+	info->flags		= FBINFO_DEFAULT;
+
+	SetCoreClockPLL(deviceInfo.pSTGReg, pdev);
+
+	deviceInfo.ulNextFreeVidMem = 0;
+	deviceInfo.ulOverlayOffset = 0;
+
+	/* This should give a reasonable default video mode */
+	if (!fb_find_mode(&info->var, info, mode_option, kyro_modedb,
+			  NUM_TOTAL_MODES, &kyro_modedb[VMODE_1024_768_75], 32))
+		info->var = kyro_var;
+
+	fb_alloc_cmap(&info->cmap, 256, 0);
+
+	kyrofb_set_par(info);
+	kyrofb_check_var(&info->var, info);
+
+	size = get_line_length(info->var.xres_virtual,
+			       info->var.bits_per_pixel);
+	size *= info->var.yres_virtual;
+
+	fb_memset(info->screen_base, 0, size);
+
+	if (register_framebuffer(info) < 0)
+		goto out_unmap;
+
+	fb_info(info, "%s frame buffer device, at %dx%d@%d using %ldk/%ldk of VRAM\n",
+		info->fix.id,
+		info->var.xres, info->var.yres, info->var.bits_per_pixel,
+		size >> 10, (unsigned long)info->fix.smem_len >> 10);
+
+	pci_set_drvdata(pdev, info);
+
+	return 0;
+
+out_unmap:
+	iounmap(currentpar->regbase);
+	iounmap(info->screen_base);
+	framebuffer_release(info);
+
+	return -EINVAL;
+}
+
+static void kyrofb_remove(struct pci_dev *pdev)
+{
+	struct fb_info *info = pci_get_drvdata(pdev);
+	struct kyrofb_info *par = info->par;
+
+	/* Reset the board */
+	StopVTG(deviceInfo.pSTGReg);
+	DisableRamdacOutput(deviceInfo.pSTGReg);
+
+	/* Sync up the PLL */
+	SetCoreClockPLL(deviceInfo.pSTGReg, pdev);
+
+	deviceInfo.ulNextFreeVidMem = 0;
+	deviceInfo.ulOverlayOffset = 0;
+
+	iounmap(info->screen_base);
+	iounmap(par->regbase);
+
+#ifdef CONFIG_MTRR
+	if (par->mtrr_handle)
+		mtrr_del(par->mtrr_handle,
+			 info->fix.smem_start,
+			 info->fix.smem_len);
+#endif
+
+	unregister_framebuffer(info);
+	framebuffer_release(info);
+}
+
+static int __init kyrofb_init(void)
+{
+#ifndef MODULE
+	char *option = NULL;
+
+	if (fb_get_options("kyrofb", &option))
+		return -ENODEV;
+	kyrofb_setup(option);
+#endif
+	return pci_register_driver(&kyrofb_pci_driver);
+}
+
+static void __exit kyrofb_exit(void)
+{
+	pci_unregister_driver(&kyrofb_pci_driver);
+}
+
+module_init(kyrofb_init);
+
+#ifdef MODULE
+module_exit(kyrofb_exit);
+#endif
+
+MODULE_AUTHOR("STMicroelectronics; Paul Mundt <lethal@linux-sh.org>");
+MODULE_LICENSE("GPL");
-- 
cgit 1.2.3-korg