xref: /openbmc/linux/drivers/video/fbdev/kyro/fbdev.c (revision 2a598d0b)
1 /*
2  *  linux/drivers/video/kyro/fbdev.c
3  *
4  *  Copyright (C) 2002 STMicroelectronics
5  *  Copyright (C) 2003, 2004 Paul Mundt
6  *
7  * This file is subject to the terms and conditions of the GNU General Public
8  * License.  See the file COPYING in the main directory of this archive
9  * for more details.
10  */
11 
12 #include <linux/aperture.h>
13 #include <linux/module.h>
14 #include <linux/types.h>
15 #include <linux/kernel.h>
16 #include <linux/mm.h>
17 #include <linux/errno.h>
18 #include <linux/string.h>
19 #include <linux/delay.h>
20 #include <linux/fb.h>
21 #include <linux/ioctl.h>
22 #include <linux/init.h>
23 #include <linux/pci.h>
24 #include <asm/io.h>
25 #include <linux/uaccess.h>
26 
27 #include <video/kyro.h>
28 
29 #include "STG4000Reg.h"
30 #include "STG4000Interface.h"
31 
32 /*
33  * PCI Definitions
34  */
35 #define PCI_VENDOR_ID_ST	0x104a
36 #define PCI_DEVICE_ID_STG4000	0x0010
37 
38 #define KHZ2PICOS(a) (1000000000UL/(a))
39 
40 /****************************************************************************/
41 static struct fb_fix_screeninfo kyro_fix = {
42 	.id		= "ST Kyro",
43 	.type		= FB_TYPE_PACKED_PIXELS,
44 	.visual		= FB_VISUAL_TRUECOLOR,
45 	.accel		= FB_ACCEL_NONE,
46 };
47 
48 static const struct fb_var_screeninfo kyro_var = {
49 	/* 640x480, 16bpp @ 60 Hz */
50 	.xres		= 640,
51 	.yres		= 480,
52 	.xres_virtual	= 640,
53 	.yres_virtual	= 480,
54 	.bits_per_pixel	= 16,
55 	.red		= { 11, 5, 0 },
56 	.green		= {  5, 6, 0 },
57 	.blue		= {  0, 5, 0 },
58 	.activate	= FB_ACTIVATE_NOW,
59 	.height		= -1,
60 	.width		= -1,
61 	.pixclock	= KHZ2PICOS(25175),
62 	.left_margin	= 48,
63 	.right_margin	= 16,
64 	.upper_margin	= 33,
65 	.lower_margin	= 10,
66 	.hsync_len	= 96,
67 	.vsync_len	= 2,
68 	.vmode		= FB_VMODE_NONINTERLACED,
69 };
70 
71 typedef struct {
72 	STG4000REG __iomem *pSTGReg;	/* Virtual address of PCI register region */
73 	u32 ulNextFreeVidMem;	/* Offset from start of vid mem to next free region */
74 	u32 ulOverlayOffset;	/* Offset from start of vid mem to overlay */
75 	u32 ulOverlayStride;	/* Interleaved YUV and 422 mode Y stride */
76 	u32 ulOverlayUVStride;	/* 422 mode U & V stride */
77 } device_info_t;
78 
79 /* global graphics card info structure (one per card) */
80 static device_info_t deviceInfo;
81 
82 static char *mode_option = NULL;
83 static int nopan = 0;
84 static int nowrap = 1;
85 static int nomtrr = 0;
86 
87 /* PCI driver prototypes */
88 static int kyrofb_probe(struct pci_dev *pdev, const struct pci_device_id *ent);
89 static void kyrofb_remove(struct pci_dev *pdev);
90 
91 static struct fb_videomode kyro_modedb[] = {
92 	{
93 		/* 640x350 @ 85Hz */
94 		NULL, 85, 640, 350, KHZ2PICOS(31500),
95 		96, 32, 60, 32, 64, 3,
96 		FB_SYNC_HOR_HIGH_ACT, FB_VMODE_NONINTERLACED
97 	}, {
98 		/* 640x400 @ 85Hz */
99 		NULL, 85, 640, 400, KHZ2PICOS(31500),
100 		96, 32, 41, 1, 64, 3,
101 		FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
102 	}, {
103 		/* 720x400 @ 85Hz */
104 		NULL, 85, 720, 400, KHZ2PICOS(35500),
105 		108, 36, 42, 1, 72, 3,
106 		FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
107 	}, {
108 		/* 640x480 @ 60Hz */
109 		NULL, 60, 640, 480, KHZ2PICOS(25175),
110 		48, 16, 33, 10, 96, 2,
111 		0, FB_VMODE_NONINTERLACED
112 	}, {
113 		/* 640x480 @ 72Hz */
114 		NULL, 72, 640, 480, KHZ2PICOS(31500),
115 		128, 24, 28, 9, 40, 3,
116 		0, FB_VMODE_NONINTERLACED
117 	}, {
118 		/* 640x480 @ 75Hz */
119 		NULL, 75, 640, 480, KHZ2PICOS(31500),
120 		120, 16, 16, 1, 64, 3,
121 		0, FB_VMODE_NONINTERLACED
122 	}, {
123 		/* 640x480 @ 85Hz */
124 		NULL, 85, 640, 480, KHZ2PICOS(36000),
125 		80, 56, 25, 1, 56, 3,
126 		0, FB_VMODE_NONINTERLACED
127 	}, {
128 		/* 800x600 @ 56Hz */
129 		NULL, 56, 800, 600, KHZ2PICOS(36000),
130 		128, 24, 22, 1, 72, 2,
131 		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
132 	}, {
133 		/* 800x600 @ 60Hz */
134 		NULL, 60, 800, 600, KHZ2PICOS(40000),
135 		88, 40, 23, 1, 128, 4,
136 		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
137 	}, {
138 		/* 800x600 @ 72Hz */
139 		NULL, 72, 800, 600, KHZ2PICOS(50000),
140 		64, 56, 23, 37, 120, 6,
141 		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
142 	}, {
143 		/* 800x600 @ 75Hz */
144 		NULL, 75, 800, 600, KHZ2PICOS(49500),
145 		160, 16, 21, 1, 80, 3,
146 		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
147 	}, {
148 		/* 800x600 @ 85Hz */
149 		NULL, 85, 800, 600, KHZ2PICOS(56250),
150 		152, 32, 27, 1, 64, 3,
151 		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
152 	}, {
153 		/* 1024x768 @ 60Hz */
154 		NULL, 60, 1024, 768, KHZ2PICOS(65000),
155 		160, 24, 29, 3, 136, 6,
156 		0, FB_VMODE_NONINTERLACED
157 	}, {
158 		/* 1024x768 @ 70Hz */
159 		NULL, 70, 1024, 768, KHZ2PICOS(75000),
160 		144, 24, 29, 3, 136, 6,
161 		0, FB_VMODE_NONINTERLACED
162 	}, {
163 		/* 1024x768 @ 75Hz */
164 		NULL, 75, 1024, 768, KHZ2PICOS(78750),
165 		176, 16, 28, 1, 96, 3,
166 		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
167 	}, {
168 		/* 1024x768 @ 85Hz */
169 		NULL, 85, 1024, 768, KHZ2PICOS(94500),
170 		208, 48, 36, 1, 96, 3,
171 		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
172 	}, {
173 		/* 1152x864 @ 75Hz */
174 		NULL, 75, 1152, 864, KHZ2PICOS(108000),
175 		256, 64, 32, 1, 128, 3,
176 		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
177 	}, {
178 		/* 1280x960 @ 60Hz */
179 		NULL, 60, 1280, 960, KHZ2PICOS(108000),
180 		312, 96, 36, 1, 112, 3,
181 		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
182 	}, {
183 		/* 1280x960 @ 85Hz */
184 		NULL, 85, 1280, 960, KHZ2PICOS(148500),
185 		224, 64, 47, 1, 160, 3,
186 		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
187 	}, {
188 		/* 1280x1024 @ 60Hz */
189 		NULL, 60, 1280, 1024, KHZ2PICOS(108000),
190 		248, 48, 38, 1, 112, 3,
191 		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
192 	}, {
193 		/* 1280x1024 @ 75Hz */
194 		NULL, 75, 1280, 1024, KHZ2PICOS(135000),
195 		248, 16, 38, 1, 144, 3,
196 		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
197 	}, {
198 		/* 1280x1024 @ 85Hz */
199 		NULL, 85, 1280, 1024, KHZ2PICOS(157500),
200 		224, 64, 44, 1, 160, 3,
201 		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
202 	}, {
203 		/* 1600x1200 @ 60Hz */
204 		NULL, 60, 1600, 1200, KHZ2PICOS(162000),
205 		304, 64, 46, 1, 192, 3,
206 		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
207 	}, {
208 		/* 1600x1200 @ 65Hz */
209 		NULL, 65, 1600, 1200, KHZ2PICOS(175500),
210 		304, 64, 46, 1, 192, 3,
211 		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
212 	}, {
213 		/* 1600x1200 @ 70Hz */
214 		NULL, 70, 1600, 1200, KHZ2PICOS(189000),
215 		304, 64, 46, 1, 192, 3,
216 		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
217 	}, {
218 		/* 1600x1200 @ 75Hz */
219 		NULL, 75, 1600, 1200, KHZ2PICOS(202500),
220 		304, 64, 46, 1, 192, 3,
221 		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
222 	}, {
223 		/* 1600x1200 @ 85Hz */
224 		NULL, 85, 1600, 1200, KHZ2PICOS(229500),
225 		304, 64, 46, 1, 192, 3,
226 		FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
227 	}, {
228 		/* 1792x1344 @ 60Hz */
229 		NULL, 60, 1792, 1344, KHZ2PICOS(204750),
230 		328, 128, 46, 1, 200, 3,
231 		FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
232 	}, {
233 		/* 1792x1344 @ 75Hz */
234 		NULL, 75, 1792, 1344, KHZ2PICOS(261000),
235 		352, 96, 69, 1, 216, 3,
236 		FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
237 	}, {
238 		/* 1856x1392 @ 60Hz */
239 		NULL, 60, 1856, 1392, KHZ2PICOS(218250),
240 		352, 96, 43, 1, 224, 3,
241 		FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
242 	}, {
243 		/* 1856x1392 @ 75Hz */
244 		NULL, 75, 1856, 1392, KHZ2PICOS(288000),
245 		352, 128, 104, 1, 224, 3,
246 		FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
247 	}, {
248 		/* 1920x1440 @ 60Hz */
249 		NULL, 60, 1920, 1440, KHZ2PICOS(234000),
250 		344, 128, 56, 1, 208, 3,
251 		FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
252 	}, {
253 		/* 1920x1440 @ 75Hz */
254 		NULL, 75, 1920, 1440, KHZ2PICOS(297000),
255 		352, 144, 56, 1, 224, 3,
256 		FB_SYNC_VERT_HIGH_ACT, FB_VMODE_NONINTERLACED
257 	},
258 };
259 #define NUM_TOTAL_MODES	ARRAY_SIZE(kyro_modedb)
260 
261 /*
262  * This needs to be kept ordered corresponding to kyro_modedb.
263  */
264 enum {
265 	VMODE_640_350_85,
266 	VMODE_640_400_85,
267 	VMODE_720_400_85,
268 	VMODE_640_480_60,
269 	VMODE_640_480_72,
270 	VMODE_640_480_75,
271 	VMODE_640_480_85,
272 	VMODE_800_600_56,
273 	VMODE_800_600_60,
274 	VMODE_800_600_72,
275 	VMODE_800_600_75,
276 	VMODE_800_600_85,
277 	VMODE_1024_768_60,
278 	VMODE_1024_768_70,
279 	VMODE_1024_768_75,
280 	VMODE_1024_768_85,
281 	VMODE_1152_864_75,
282 	VMODE_1280_960_60,
283 	VMODE_1280_960_85,
284 	VMODE_1280_1024_60,
285 	VMODE_1280_1024_75,
286 	VMODE_1280_1024_85,
287 	VMODE_1600_1200_60,
288 	VMODE_1600_1200_65,
289 	VMODE_1600_1200_70,
290 	VMODE_1600_1200_75,
291 	VMODE_1600_1200_85,
292 	VMODE_1792_1344_60,
293 	VMODE_1792_1344_75,
294 	VMODE_1856_1392_60,
295 	VMODE_1856_1392_75,
296 	VMODE_1920_1440_60,
297 	VMODE_1920_1440_75,
298 };
299 
300 /* Accessors */
301 static int kyro_dev_video_mode_set(struct fb_info *info)
302 {
303 	struct kyrofb_info *par = info->par;
304 
305 	/* Turn off display */
306 	StopVTG(deviceInfo.pSTGReg);
307 	DisableRamdacOutput(deviceInfo.pSTGReg);
308 
309 	/* Bring us out of VGA and into Hi-Res mode, if not already. */
310 	DisableVGA(deviceInfo.pSTGReg);
311 
312 	if (InitialiseRamdac(deviceInfo.pSTGReg,
313 			     info->var.bits_per_pixel,
314 			     info->var.xres, info->var.yres,
315 			     par->HSP, par->VSP, &par->PIXCLK) < 0)
316 		return -EINVAL;
317 
318 	SetupVTG(deviceInfo.pSTGReg, par);
319 
320 	ResetOverlayRegisters(deviceInfo.pSTGReg);
321 
322 	/* Turn on display in new mode */
323 	EnableRamdacOutput(deviceInfo.pSTGReg);
324 	StartVTG(deviceInfo.pSTGReg);
325 
326 	deviceInfo.ulNextFreeVidMem = info->var.xres * info->var.yres *
327 				      info->var.bits_per_pixel;
328 	deviceInfo.ulOverlayOffset = 0;
329 
330 	return 0;
331 }
332 
333 static int kyro_dev_overlay_create(u32 ulWidth,
334 				   u32 ulHeight, int bLinear)
335 {
336 	u32 offset;
337 	u32 stride, uvStride;
338 
339 	if (deviceInfo.ulOverlayOffset != 0)
340 		/*
341 		 * Can only create one overlay without resetting the card or
342 		 * changing display mode
343 		 */
344 		return -EINVAL;
345 
346 	ResetOverlayRegisters(deviceInfo.pSTGReg);
347 
348 	/* Overlays are addressed in multiples of 16bytes or 32bytes, so make
349 	 * sure the start offset is on an appropriate boundary.
350 	 */
351 	offset = deviceInfo.ulNextFreeVidMem;
352 	if ((offset & 0x1f) != 0) {
353 		offset = (offset + 32L) & 0xffffffE0L;
354 	}
355 
356 	if (CreateOverlaySurface(deviceInfo.pSTGReg, ulWidth, ulHeight,
357 				 bLinear, offset, &stride, &uvStride) < 0)
358 		return -EINVAL;
359 
360 	deviceInfo.ulOverlayOffset = offset;
361 	deviceInfo.ulOverlayStride = stride;
362 	deviceInfo.ulOverlayUVStride = uvStride;
363 	deviceInfo.ulNextFreeVidMem = offset + (ulHeight * stride) + (ulHeight * 2 * uvStride);
364 
365 	SetOverlayBlendMode(deviceInfo.pSTGReg, GLOBAL_ALPHA, 0xf, 0x0);
366 
367 	return 0;
368 }
369 
370 static int kyro_dev_overlay_viewport_set(u32 x, u32 y, u32 ulWidth, u32 ulHeight)
371 {
372 	if (deviceInfo.ulOverlayOffset == 0)
373 		/* probably haven't called CreateOverlay yet */
374 		return -EINVAL;
375 
376 	if (ulWidth == 0 || ulWidth == 0xffffffff ||
377 	    ulHeight == 0 || ulHeight == 0xffffffff ||
378 	    (x < 2 && ulWidth + 2 == 0))
379 		return -EINVAL;
380 
381 	/* Stop Ramdac Output */
382 	DisableRamdacOutput(deviceInfo.pSTGReg);
383 
384 	SetOverlayViewPort(deviceInfo.pSTGReg,
385 			   x, y, x + ulWidth - 1, y + ulHeight - 1);
386 
387 	EnableOverlayPlane(deviceInfo.pSTGReg);
388 	/* Start Ramdac Output */
389 	EnableRamdacOutput(deviceInfo.pSTGReg);
390 
391 	return 0;
392 }
393 
394 static inline unsigned long get_line_length(int x, int bpp)
395 {
396 	return (unsigned long)((((x*bpp)+31)&~31) >> 3);
397 }
398 
399 static int kyrofb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
400 {
401 	struct kyrofb_info *par = info->par;
402 
403 	if (!var->pixclock)
404 		return -EINVAL;
405 
406 	if (var->bits_per_pixel != 16 && var->bits_per_pixel != 32) {
407 		printk(KERN_WARNING "kyrofb: depth not supported: %u\n", var->bits_per_pixel);
408 		return -EINVAL;
409 	}
410 
411 	switch (var->bits_per_pixel) {
412 	case 16:
413 		var->red.offset = 11;
414 		var->red.length = 5;
415 		var->green.offset = 5;
416 		var->green.length = 6;
417 		var->blue.length = 5;
418 		break;
419 	case 32:
420 		var->transp.offset = 24;
421 		var->red.offset = 16;
422 		var->green.offset = 8;
423 		var->blue.offset = 0;
424 
425 		var->red.length = 8;
426 		var->green.length = 8;
427 		var->blue.length = 8;
428 		var->transp.length = 8;
429 		break;
430 	}
431 
432 	/* Height/Width of picture in mm */
433 	var->height = var->width = -1;
434 
435 	/* Timing information. All values are in picoseconds */
436 
437 	/* par->PIXCLK is in 100Hz units. Convert to picoseconds -
438 	 * ensuring we do not exceed 32 bit precision
439 	 */
440 	/*
441 	 * XXX: Enabling this really screws over the pixclock value when we
442 	 * read it back with fbset. As such, leaving this commented out appears
443 	 * to do the right thing (at least for now) .. bearing in mind that we
444 	 * have infact already done the KHZ2PICOS conversion in both the modedb
445 	 * and kyro_var. -- PFM.
446 	 */
447 //	var->pixclock = 1000000000 / (par->PIXCLK / 10);
448 
449 	/* the header file claims we should use picoseconds
450 	 * - nobody else does though, the all use pixels and lines
451 	 * of h and v sizes. Both options here.
452 	 */
453 
454 	/*
455 	 * If we're being called by __fb_try_mode(), then we don't want to
456 	 * override any of the var settings that we've already parsed
457 	 * from our modedb. -- PFM.
458 	 */
459 	if ((var->activate & FB_ACTIVATE_MASK) == FB_ACTIVATE_TEST)
460 		return 0;
461 
462 	var->left_margin = par->HBP;
463 	var->hsync_len = par->HST;
464 	var->right_margin = par->HFP;
465 
466 	var->upper_margin = par->VBP;
467 	var->vsync_len = par->VST;
468 	var->lower_margin = par->VFP;
469 
470 	if (par->HSP == 1)
471 		var->sync |= FB_SYNC_HOR_HIGH_ACT;
472 	if (par->VSP == 1)
473 		var->sync |= FB_SYNC_VERT_HIGH_ACT;
474 
475 	return 0;
476 }
477 
478 static int kyrofb_set_par(struct fb_info *info)
479 {
480 	struct kyrofb_info *par = info->par;
481 	unsigned long lineclock;
482 	unsigned long frameclock;
483 
484 	/* Actual resolution */
485 	par->XRES = info->var.xres;
486 	par->YRES = info->var.yres;
487 
488 	/* pixel depth */
489 	par->PIXDEPTH = info->var.bits_per_pixel;
490 
491 	/* Refresh rate */
492 	/* time for a line in ns */
493 	lineclock = (info->var.pixclock * (info->var.xres +
494 				    info->var.right_margin +
495 				    info->var.hsync_len +
496 				    info->var.left_margin)) / 1000;
497 
498 	if (!lineclock)
499 		return -EINVAL;
500 
501 	/* time for a frame in ns (precision in 32bpp) */
502 	frameclock = lineclock * (info->var.yres +
503 				  info->var.lower_margin +
504 				  info->var.vsync_len +
505 				  info->var.upper_margin);
506 
507 	/* Calculate refresh rate and horrizontal clocks */
508 	par->VFREQ = (1000000000 + (frameclock / 2)) / frameclock;
509 	par->HCLK = (1000000000 + (lineclock / 2)) / lineclock;
510 	par->PIXCLK = ((1000000000 + (info->var.pixclock / 2))
511 					/ info->var.pixclock) * 10;
512 
513 	/* calculate horizontal timings */
514 	par->HFP = info->var.right_margin;
515 	par->HST = info->var.hsync_len;
516 	par->HBP = info->var.left_margin;
517 	par->HTot = par->XRES + par->HBP + par->HST + par->HFP;
518 
519 	/* calculate vertical timings */
520 	par->VFP = info->var.lower_margin;
521 	par->VST = info->var.vsync_len;
522 	par->VBP = info->var.upper_margin;
523 	par->VTot = par->YRES + par->VBP + par->VST + par->VFP;
524 
525 	par->HSP = (info->var.sync & FB_SYNC_HOR_HIGH_ACT) ? 1 : 0;
526 	par->VSP = (info->var.sync & FB_SYNC_VERT_HIGH_ACT) ? 1 : 0;
527 
528 	kyro_dev_video_mode_set(info);
529 
530 	/* length of a line in bytes    */
531 	info->fix.line_length = get_line_length(par->XRES, par->PIXDEPTH);
532 	info->fix.visual = FB_VISUAL_TRUECOLOR;
533 
534 	return 0;
535 }
536 
537 static int kyrofb_setcolreg(u_int regno, u_int red, u_int green,
538 			    u_int blue, u_int transp, struct fb_info *info)
539 {
540 	struct kyrofb_info *par = info->par;
541 
542 	if (regno > 255)
543 		return 1;	/* Invalid register */
544 
545 	if (regno < 16) {
546 		switch (info->var.bits_per_pixel) {
547 		case 16:
548 			par->palette[regno] =
549 			     (red   & 0xf800) |
550 			    ((green & 0xfc00) >> 5) |
551 			    ((blue  & 0xf800) >> 11);
552 			break;
553 		case 32:
554 			red >>= 8; green >>= 8; blue >>= 8; transp >>= 8;
555 			par->palette[regno] =
556 			    (transp << 24) | (red << 16) | (green << 8) | blue;
557 			break;
558 		}
559 	}
560 
561 	return 0;
562 }
563 
564 #ifndef MODULE
565 static int __init kyrofb_setup(char *options)
566 {
567 	char *this_opt;
568 
569 	if (!options || !*options)
570 		return 0;
571 
572 	while ((this_opt = strsep(&options, ","))) {
573 		if (!*this_opt)
574 			continue;
575 		if (strcmp(this_opt, "nopan") == 0) {
576 			nopan = 1;
577 		} else if (strcmp(this_opt, "nowrap") == 0) {
578 			nowrap = 1;
579 		} else if (strcmp(this_opt, "nomtrr") == 0) {
580 			nomtrr = 1;
581 		} else {
582 			mode_option = this_opt;
583 		}
584 	}
585 
586 	return 0;
587 }
588 #endif
589 
590 static int kyrofb_ioctl(struct fb_info *info,
591 			unsigned int cmd, unsigned long arg)
592 {
593 	overlay_create ol_create;
594 	overlay_viewport_set ol_viewport_set;
595 	void __user *argp = (void __user *)arg;
596 
597 	switch (cmd) {
598 	case KYRO_IOCTL_OVERLAY_CREATE:
599 		if (copy_from_user(&ol_create, argp, sizeof(overlay_create)))
600 			return -EFAULT;
601 
602 		if (kyro_dev_overlay_create(ol_create.ulWidth,
603 					    ol_create.ulHeight, 0) < 0) {
604 			printk(KERN_ERR "Kyro FB: failed to create overlay surface.\n");
605 
606 			return -EINVAL;
607 		}
608 		break;
609 	case KYRO_IOCTL_OVERLAY_VIEWPORT_SET:
610 		if (copy_from_user(&ol_viewport_set, argp,
611 			       sizeof(overlay_viewport_set)))
612 			return -EFAULT;
613 
614 		if (kyro_dev_overlay_viewport_set(ol_viewport_set.xOrgin,
615 						  ol_viewport_set.yOrgin,
616 						  ol_viewport_set.xSize,
617 						  ol_viewport_set.ySize) != 0)
618 		{
619 			printk(KERN_ERR "Kyro FB: failed to create overlay viewport.\n");
620 			return -EINVAL;
621 		}
622 		break;
623 	case KYRO_IOCTL_SET_VIDEO_MODE:
624 		{
625 			printk(KERN_ERR "Kyro FB: KYRO_IOCTL_SET_VIDEO_MODE is"
626 				"obsolete, use the appropriate fb_ioctl()"
627 				"command instead.\n");
628 			return -EINVAL;
629 		}
630 	case KYRO_IOCTL_UVSTRIDE:
631 		if (copy_to_user(argp, &deviceInfo.ulOverlayUVStride, sizeof(deviceInfo.ulOverlayUVStride)))
632 			return -EFAULT;
633 		break;
634 	case KYRO_IOCTL_STRIDE:
635 		if (copy_to_user(argp, &deviceInfo.ulOverlayStride, sizeof(deviceInfo.ulOverlayStride)))
636 			return -EFAULT;
637 		break;
638 	case KYRO_IOCTL_OVERLAY_OFFSET:
639 		if (copy_to_user(argp, &deviceInfo.ulOverlayOffset, sizeof(deviceInfo.ulOverlayOffset)))
640 			return -EFAULT;
641 		break;
642 	}
643 
644 	return 0;
645 }
646 
647 static const struct pci_device_id kyrofb_pci_tbl[] = {
648 	{ PCI_VENDOR_ID_ST, PCI_DEVICE_ID_STG4000,
649 	  PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0 },
650 	{ 0, }
651 };
652 
653 MODULE_DEVICE_TABLE(pci, kyrofb_pci_tbl);
654 
655 static struct pci_driver kyrofb_pci_driver = {
656 	.name		= "kyrofb",
657 	.id_table	= kyrofb_pci_tbl,
658 	.probe		= kyrofb_probe,
659 	.remove		= kyrofb_remove,
660 };
661 
662 static const struct fb_ops kyrofb_ops = {
663 	.owner		= THIS_MODULE,
664 	.fb_check_var	= kyrofb_check_var,
665 	.fb_set_par	= kyrofb_set_par,
666 	.fb_setcolreg	= kyrofb_setcolreg,
667 	.fb_ioctl	= kyrofb_ioctl,
668 	.fb_fillrect	= cfb_fillrect,
669 	.fb_copyarea	= cfb_copyarea,
670 	.fb_imageblit	= cfb_imageblit,
671 };
672 
673 static int kyrofb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
674 {
675 	struct fb_info *info;
676 	struct kyrofb_info *currentpar;
677 	unsigned long size;
678 	int err;
679 
680 	err = aperture_remove_conflicting_pci_devices(pdev, "kyrofb");
681 	if (err)
682 		return err;
683 
684 	if ((err = pci_enable_device(pdev))) {
685 		printk(KERN_WARNING "kyrofb: Can't enable pdev: %d\n", err);
686 		return err;
687 	}
688 
689 	info = framebuffer_alloc(sizeof(struct kyrofb_info), &pdev->dev);
690 	if (!info)
691 		return -ENOMEM;
692 
693 	currentpar = info->par;
694 
695 	kyro_fix.smem_start = pci_resource_start(pdev, 0);
696 	kyro_fix.smem_len   = pci_resource_len(pdev, 0);
697 	kyro_fix.mmio_start = pci_resource_start(pdev, 1);
698 	kyro_fix.mmio_len   = pci_resource_len(pdev, 1);
699 
700 	currentpar->regbase = deviceInfo.pSTGReg =
701 		ioremap(kyro_fix.mmio_start, kyro_fix.mmio_len);
702 	if (!currentpar->regbase)
703 		goto out_free_fb;
704 
705 	info->screen_base = pci_ioremap_wc_bar(pdev, 0);
706 	if (!info->screen_base)
707 		goto out_unmap_regs;
708 
709 	if (!nomtrr)
710 		currentpar->wc_cookie = arch_phys_wc_add(kyro_fix.smem_start,
711 							 kyro_fix.smem_len);
712 
713 	kyro_fix.ypanstep	= nopan ? 0 : 1;
714 	kyro_fix.ywrapstep	= nowrap ? 0 : 1;
715 
716 	info->fbops		= &kyrofb_ops;
717 	info->fix		= kyro_fix;
718 	info->pseudo_palette	= currentpar->palette;
719 	info->flags		= FBINFO_DEFAULT;
720 
721 	SetCoreClockPLL(deviceInfo.pSTGReg, pdev);
722 
723 	deviceInfo.ulNextFreeVidMem = 0;
724 	deviceInfo.ulOverlayOffset = 0;
725 
726 	/* This should give a reasonable default video mode */
727 	if (!fb_find_mode(&info->var, info, mode_option, kyro_modedb,
728 			  NUM_TOTAL_MODES, &kyro_modedb[VMODE_1024_768_75], 32))
729 		info->var = kyro_var;
730 
731 	fb_alloc_cmap(&info->cmap, 256, 0);
732 
733 	kyrofb_set_par(info);
734 	kyrofb_check_var(&info->var, info);
735 
736 	size = get_line_length(info->var.xres_virtual,
737 			       info->var.bits_per_pixel);
738 	size *= info->var.yres_virtual;
739 
740 	fb_memset_io(info->screen_base, 0, size);
741 
742 	if (register_framebuffer(info) < 0)
743 		goto out_unmap;
744 
745 	fb_info(info, "%s frame buffer device, at %dx%d@%d using %ldk/%ldk of VRAM\n",
746 		info->fix.id,
747 		info->var.xres, info->var.yres, info->var.bits_per_pixel,
748 		size >> 10, (unsigned long)info->fix.smem_len >> 10);
749 
750 	pci_set_drvdata(pdev, info);
751 
752 	return 0;
753 
754 out_unmap:
755 	iounmap(info->screen_base);
756 out_unmap_regs:
757 	iounmap(currentpar->regbase);
758 out_free_fb:
759 	framebuffer_release(info);
760 
761 	return -EINVAL;
762 }
763 
764 static void kyrofb_remove(struct pci_dev *pdev)
765 {
766 	struct fb_info *info = pci_get_drvdata(pdev);
767 	struct kyrofb_info *par = info->par;
768 
769 	/* Reset the board */
770 	StopVTG(deviceInfo.pSTGReg);
771 	DisableRamdacOutput(deviceInfo.pSTGReg);
772 
773 	/* Sync up the PLL */
774 	SetCoreClockPLL(deviceInfo.pSTGReg, pdev);
775 
776 	deviceInfo.ulNextFreeVidMem = 0;
777 	deviceInfo.ulOverlayOffset = 0;
778 
779 	iounmap(info->screen_base);
780 	iounmap(par->regbase);
781 
782 	arch_phys_wc_del(par->wc_cookie);
783 
784 	unregister_framebuffer(info);
785 	framebuffer_release(info);
786 }
787 
788 static int __init kyrofb_init(void)
789 {
790 #ifndef MODULE
791 	char *option = NULL;
792 #endif
793 
794 	if (fb_modesetting_disabled("kyrofb"))
795 		return -ENODEV;
796 
797 #ifndef MODULE
798 	if (fb_get_options("kyrofb", &option))
799 		return -ENODEV;
800 	kyrofb_setup(option);
801 #endif
802 	return pci_register_driver(&kyrofb_pci_driver);
803 }
804 
805 static void __exit kyrofb_exit(void)
806 {
807 	pci_unregister_driver(&kyrofb_pci_driver);
808 }
809 
810 module_init(kyrofb_init);
811 
812 #ifdef MODULE
813 module_exit(kyrofb_exit);
814 #endif
815 
816 MODULE_AUTHOR("STMicroelectronics; Paul Mundt <lethal@linux-sh.org>");
817 MODULE_LICENSE("GPL");
818