hdaps: Handle errors from input_register_device
[linux-2.6] / drivers / video / S3triofb.c
1 /*
2  *  linux/drivers/video/S3Triofb.c -- Open Firmware based frame buffer device
3  *
4  *      Copyright (C) 1997 Peter De Schrijver
5  *
6  *  This driver is partly based on the PowerMac console driver:
7  *
8  *      Copyright (C) 1996 Paul Mackerras
9  *
10  *  and on the Open Firmware based frame buffer device:
11  *
12  *      Copyright (C) 1997 Geert Uytterhoeven
13  *
14  *  This file is subject to the terms and conditions of the GNU General Public
15  *  License. See the file COPYING in the main directory of this archive for
16  *  more details.
17  */
18
19 /*
20         Bugs : + OF dependencies should be removed.
21                + This driver should be merged with the CyberVision driver. The
22                  CyberVision is a Zorro III implementation of the S3Trio64 chip.
23
24 */
25
26 #include <linux/kernel.h>
27 #include <linux/module.h>
28 #include <linux/errno.h>
29 #include <linux/string.h>
30 #include <linux/mm.h>
31 #include <linux/slab.h>
32 #include <linux/vmalloc.h>
33 #include <linux/delay.h>
34 #include <linux/interrupt.h>
35 #include <linux/fb.h>
36 #include <linux/init.h>
37 #include <linux/selection.h>
38 #include <asm/io.h>
39 #include <asm/prom.h>
40 #include <asm/pci-bridge.h>
41 #include <linux/pci.h>
42
43 #include <video/fbcon.h>
44 #include <video/fbcon-cfb8.h>
45 #include <video/s3blit.h>
46
47
48 #define mem_in8(addr)           in_8((void *)(addr))
49 #define mem_in16(addr)          in_le16((void *)(addr))
50 #define mem_in32(addr)          in_le32((void *)(addr))
51
52 #define mem_out8(val, addr)     out_8((void *)(addr), val)
53 #define mem_out16(val, addr)    out_le16((void *)(addr), val)
54 #define mem_out32(val, addr)    out_le32((void *)(addr), val)
55
56 #define IO_OUT16VAL(v, r)       (((v) << 8) | (r))
57
58 static struct display disp;
59 static struct fb_info fb_info;
60 static struct { u_char red, green, blue, pad; } palette[256];
61 static char s3trio_name[16] = "S3Trio ";
62 static char *s3trio_base;
63
64 static struct fb_fix_screeninfo fb_fix;
65 static struct fb_var_screeninfo fb_var = { 0, };
66
67
68     /*
69      *  Interface used by the world
70      */
71
72 static void __init s3triofb_of_init(struct device_node *dp);
73 static int s3trio_get_fix(struct fb_fix_screeninfo *fix, int con,
74                           struct fb_info *info);
75 static int s3trio_get_var(struct fb_var_screeninfo *var, int con,
76                           struct fb_info *info);
77 static int s3trio_set_var(struct fb_var_screeninfo *var, int con,
78                           struct fb_info *info);
79 static int s3trio_get_cmap(struct fb_cmap *cmap, int kspc, int con,
80                            struct fb_info *info);
81 static int s3trio_setcolreg(u_int regno, u_int red, u_int green, u_int blue,
82                          u_int transp, struct fb_info *info);
83 static int s3trio_pan_display(struct fb_var_screeninfo *var, int con,
84                               struct fb_info *info);
85 static void s3triofb_blank(int blank, struct fb_info *info);
86
87     /*
88      *  Interface to the low level console driver
89      */
90
91 int s3triofb_init(void);
92 static int s3triofbcon_switch(int con, struct fb_info *info);
93 static int s3triofbcon_updatevar(int con, struct fb_info *info);
94
95     /*
96      *  Text console acceleration
97      */
98
99 #ifdef FBCON_HAS_CFB8
100 static struct display_switch fbcon_trio8;
101 #endif
102
103     /*
104      *    Accelerated Functions used by the low level console driver
105      */
106
107 static void Trio_WaitQueue(u_short fifo);
108 static void Trio_WaitBlit(void);
109 static void Trio_BitBLT(u_short curx, u_short cury, u_short destx,
110                         u_short desty, u_short width, u_short height,
111                         u_short mode);
112 static void Trio_RectFill(u_short x, u_short y, u_short width, u_short height,
113                           u_short mode, u_short color);
114 static void Trio_MoveCursor(u_short x, u_short y);
115
116
117     /*
118      *  Internal routines
119      */
120
121 static int s3trio_getcolreg(u_int regno, u_int *red, u_int *green, u_int *blue,
122                          u_int *transp, struct fb_info *info);
123
124 static struct fb_ops s3trio_ops = {
125         .owner =        THIS_MODULE,
126         .fb_get_fix =   s3trio_get_fix,
127         .fb_get_var =   s3trio_get_var,
128         .fb_set_var =   s3trio_set_var,
129         .fb_get_cmap =  s3trio_get_cmap,
130         .fb_set_cmap =  gen_set_cmap,
131         .fb_setcolreg = s3trio_setcolreg,
132         .fb_pan_display =s3trio_pan_display,
133         .fb_blank =     s3triofb_blank,
134 };
135
136     /*
137      *  Get the Fixed Part of the Display
138      */
139
140 static int s3trio_get_fix(struct fb_fix_screeninfo *fix, int con,
141                           struct fb_info *info)
142 {
143     memcpy(fix, &fb_fix, sizeof(fb_fix));
144     return 0;
145 }
146
147
148     /*
149      *  Get the User Defined Part of the Display
150      */
151
152 static int s3trio_get_var(struct fb_var_screeninfo *var, int con,
153                           struct fb_info *info)
154 {
155     memcpy(var, &fb_var, sizeof(fb_var));
156     return 0;
157 }
158
159
160     /*
161      *  Set the User Defined Part of the Display
162      */
163
164 static int s3trio_set_var(struct fb_var_screeninfo *var, int con,
165                           struct fb_info *info)
166 {
167     if (var->xres > fb_var.xres || var->yres > fb_var.yres ||
168         var->bits_per_pixel > fb_var.bits_per_pixel )
169         /* || var->nonstd || var->vmode != FB_VMODE_NONINTERLACED) */
170         return -EINVAL;
171     if (var->xres_virtual > fb_var.xres_virtual) {
172         outw(IO_OUT16VAL((var->xres_virtual /8) & 0xff, 0x13), 0x3d4);
173         outw(IO_OUT16VAL(((var->xres_virtual /8 ) & 0x300) >> 3, 0x51), 0x3d4);
174         fb_var.xres_virtual = var->xres_virtual;
175         fb_fix.line_length = var->xres_virtual;
176     }
177     fb_var.yres_virtual = var->yres_virtual;
178     memcpy(var, &fb_var, sizeof(fb_var));
179     return 0;
180 }
181
182
183     /*
184      *  Pan or Wrap the Display
185      *
186      *  This call looks only at xoffset, yoffset and the FB_VMODE_YWRAP flag
187      */
188
189 static int s3trio_pan_display(struct fb_var_screeninfo *var, int con,
190                               struct fb_info *info)
191 {
192     unsigned int base;
193
194     if (var->xoffset > (var->xres_virtual - var->xres))
195         return -EINVAL;
196     if (var->yoffset > (var->yres_virtual - var->yres))
197         return -EINVAL;
198
199     fb_var.xoffset = var->xoffset;
200     fb_var.yoffset = var->yoffset;
201
202     base = var->yoffset * fb_fix.line_length + var->xoffset;
203
204     outw(IO_OUT16VAL((base >> 8) & 0xff, 0x0c),0x03D4);
205     outw(IO_OUT16VAL(base  & 0xff, 0x0d),0x03D4);
206     outw(IO_OUT16VAL((base >> 16) & 0xf, 0x69),0x03D4);
207     return 0;
208 }
209
210
211     /*
212      *  Get the Colormap
213      */
214
215 static int s3trio_get_cmap(struct fb_cmap *cmap, int kspc, int con,
216                            struct fb_info *info)
217 {
218     if (con == info->currcon) /* current console? */
219         return fb_get_cmap(cmap, kspc, s3trio_getcolreg, info);
220     else if (fb_display[con].cmap.len) /* non default colormap? */
221         fb_copy_cmap(&fb_display[con].cmap, cmap, kspc ? 0 : 2);
222     else
223         fb_copy_cmap(fb_default_cmap(1 << fb_display[con].var.bits_per_pixel),
224                      cmap, kspc ? 0 : 2);
225     return 0;
226 }
227
228 int __init s3triofb_init(void)
229 {
230         struct device_node *dp;
231
232         dp = find_devices("S3Trio");
233         if (dp != 0)
234             s3triofb_of_init(dp);
235         return 0;
236 }
237
238 void __init s3trio_resetaccel(void){
239
240
241 #define EC01_ENH_ENB    0x0005
242 #define EC01_LAW_ENB    0x0010
243 #define EC01_MMIO_ENB   0x0020
244
245 #define EC00_RESET      0x8000
246 #define EC00_ENABLE     0x4000
247 #define MF_MULT_MISC    0xE000
248 #define SRC_FOREGROUND  0x0020
249 #define SRC_BACKGROUND  0x0000
250 #define MIX_SRC                 0x0007
251 #define MF_T_CLIP       0x1000
252 #define MF_L_CLIP       0x2000
253 #define MF_B_CLIP       0x3000
254 #define MF_R_CLIP       0x4000
255 #define MF_PIX_CONTROL  0xA000
256 #define MFA_SRC_FOREGR_MIX      0x0000
257 #define MF_PIX_CONTROL  0xA000
258
259         outw(EC00_RESET,  0x42e8);
260         inw(  0x42e8);
261         outw(EC00_ENABLE,  0x42e8);
262         inw(  0x42e8);
263         outw(EC01_ENH_ENB | EC01_LAW_ENB,
264                    0x4ae8);
265         outw(MF_MULT_MISC,  0xbee8); /* 16 bit I/O registers */
266
267         /* Now set some basic accelerator registers */
268         Trio_WaitQueue(0x0400);
269         outw(SRC_FOREGROUND | MIX_SRC, 0xbae8);
270         outw(SRC_BACKGROUND | MIX_SRC,  0xb6e8);/* direct color*/
271         outw(MF_T_CLIP | 0, 0xbee8 );     /* clip virtual area  */
272         outw(MF_L_CLIP | 0, 0xbee8 );
273         outw(MF_R_CLIP | (640 - 1), 0xbee8);
274         outw(MF_B_CLIP | (480 - 1),  0xbee8);
275         Trio_WaitQueue(0x0400);
276         outw(0xffff,  0xaae8);       /* Enable all planes */
277         outw(0xffff, 0xaae8);       /* Enable all planes */
278         outw( MF_PIX_CONTROL | MFA_SRC_FOREGR_MIX,  0xbee8);
279 }
280
281 int __init s3trio_init(struct device_node *dp){
282
283     u_char bus, dev;
284     unsigned int t32;
285     unsigned short cmd;
286
287         pci_device_loc(dp,&bus,&dev);
288                 pcibios_read_config_dword(bus, dev, PCI_VENDOR_ID, &t32);
289                 if(t32 == (PCI_DEVICE_ID_S3_TRIO << 16) + PCI_VENDOR_ID_S3) {
290                         pcibios_read_config_dword(bus, dev, PCI_BASE_ADDRESS_0, &t32);
291                         pcibios_read_config_dword(bus, dev, PCI_BASE_ADDRESS_1, &t32);
292                         pcibios_read_config_word(bus, dev, PCI_COMMAND,&cmd);
293
294                         pcibios_write_config_word(bus, dev, PCI_COMMAND, PCI_COMMAND_IO | PCI_COMMAND_MEMORY);
295
296                         pcibios_write_config_dword(bus, dev, PCI_BASE_ADDRESS_0,0xffffffff);
297                         pcibios_read_config_dword(bus, dev, PCI_BASE_ADDRESS_0, &t32);
298
299 /* This is a gross hack as OF only maps enough memory for the framebuffer and
300    we want to use MMIO too. We should find out which chunk of address space
301    we can use here */
302                         pcibios_write_config_dword(bus,dev,PCI_BASE_ADDRESS_0,0xc6000000);
303
304                         /* unlock s3 */
305
306                         outb(0x01, 0x3C3);
307
308                         outb(inb(0x03CC) | 1, 0x3c2);
309
310                         outw(IO_OUT16VAL(0x48, 0x38),0x03D4);
311                         outw(IO_OUT16VAL(0xA0, 0x39),0x03D4);
312                         outb(0x33,0x3d4);
313                         outw(IO_OUT16VAL((inb(0x3d5) & ~(0x2 | 0x10 |  0x40)) |
314                                           0x20, 0x33), 0x3d4);
315
316                         outw(IO_OUT16VAL(0x6, 0x8), 0x3c4);
317
318                         /* switch to MMIO only mode */
319
320                         outb(0x58, 0x3d4);
321                         outw(IO_OUT16VAL(inb(0x3d5) | 3 | 0x10, 0x58), 0x3d4);
322                         outw(IO_OUT16VAL(8, 0x53), 0x3d4);
323
324                         /* switch off I/O accesses */
325
326 #if 0
327                         pcibios_write_config_word(bus, dev, PCI_COMMAND,
328                                         PCI_COMMAND_IO | PCI_COMMAND_MEMORY);
329 #endif
330                         return 1;
331                 }
332
333         return 0;
334 }
335
336
337     /*
338      *  Initialisation
339      *  We heavily rely on OF for the moment. This needs fixing.
340      */
341
342 static void __init s3triofb_of_init(struct device_node *dp)
343 {
344     int i, *pp, len;
345     unsigned long address, size;
346     u_long *CursorBase;
347
348     strncat(s3trio_name, dp->name, sizeof(s3trio_name));
349     s3trio_name[sizeof(s3trio_name)-1] = '\0';
350     strcpy(fb_fix.id, s3trio_name);
351
352     if((pp = get_property(dp, "vendor-id", &len)) != NULL
353         && *pp!=PCI_VENDOR_ID_S3) {
354         printk("%s: can't find S3 Trio board\n", dp->full_name);
355         return;
356     }
357
358     if((pp = get_property(dp, "device-id", &len)) != NULL
359         && *pp!=PCI_DEVICE_ID_S3_TRIO) {
360         printk("%s: can't find S3 Trio board\n", dp->full_name);
361         return;
362     }
363
364     if ((pp = get_property(dp, "depth", &len)) != NULL
365         && len == sizeof(int) && *pp != 8) {
366         printk("%s: can't use depth = %d\n", dp->full_name, *pp);
367         return;
368     }
369     if ((pp = get_property(dp, "width", &len)) != NULL
370         && len == sizeof(int))
371         fb_var.xres = fb_var.xres_virtual = *pp;
372     if ((pp = get_property(dp, "height", &len)) != NULL
373         && len == sizeof(int))
374         fb_var.yres = fb_var.yres_virtual = *pp;
375     if ((pp = get_property(dp, "linebytes", &len)) != NULL
376         && len == sizeof(int))
377         fb_fix.line_length = *pp;
378     else
379         fb_fix.line_length = fb_var.xres_virtual;
380     fb_fix.smem_len = fb_fix.line_length*fb_var.yres;
381
382     address = 0xc6000000;
383     size = 64*1024*1024;
384     if (!request_mem_region(address, size, "S3triofb"))
385         return;
386
387     s3trio_init(dp);
388     s3trio_base = ioremap(address, size);
389     fb_fix.smem_start = address;
390     fb_fix.type = FB_TYPE_PACKED_PIXELS;
391     fb_fix.type_aux = 0;
392     fb_fix.accel = FB_ACCEL_S3_TRIO64;
393     fb_fix.mmio_start = address+0x1000000;
394     fb_fix.mmio_len = 0x1000000;
395
396     fb_fix.xpanstep = 1;
397     fb_fix.ypanstep = 1;
398
399     s3trio_resetaccel();
400
401     mem_out8(0x30, s3trio_base+0x1008000 + 0x03D4);
402     mem_out8(0x2d, s3trio_base+0x1008000 + 0x03D4);
403     mem_out8(0x2e, s3trio_base+0x1008000 + 0x03D4);
404
405     mem_out8(0x50, s3trio_base+0x1008000 + 0x03D4);
406
407     /* disable HW cursor */
408
409     mem_out8(0x39, s3trio_base+0x1008000 + 0x03D4);
410     mem_out8(0xa0, s3trio_base+0x1008000 + 0x03D5);
411
412     mem_out8(0x45, s3trio_base+0x1008000 + 0x03D4);
413     mem_out8(0, s3trio_base+0x1008000 + 0x03D5);
414
415     mem_out8(0x4e, s3trio_base+0x1008000 + 0x03D4);
416     mem_out8(0, s3trio_base+0x1008000 + 0x03D5);
417
418     mem_out8(0x4f, s3trio_base+0x1008000 + 0x03D4);
419     mem_out8(0, s3trio_base+0x1008000 + 0x03D5);
420
421     /* init HW cursor */
422
423     CursorBase = (u_long *)(s3trio_base + 2*1024*1024 - 0x400);
424         for (i = 0; i < 8; i++) {
425                 *(CursorBase  +(i*4)) = 0xffffff00;
426                 *(CursorBase+1+(i*4)) = 0xffff0000;
427                 *(CursorBase+2+(i*4)) = 0xffff0000;
428                 *(CursorBase+3+(i*4)) = 0xffff0000;
429         }
430         for (i = 8; i < 64; i++) {
431                 *(CursorBase  +(i*4)) = 0xffff0000;
432                 *(CursorBase+1+(i*4)) = 0xffff0000;
433                 *(CursorBase+2+(i*4)) = 0xffff0000;
434                 *(CursorBase+3+(i*4)) = 0xffff0000;
435         }
436
437
438     mem_out8(0x4c, s3trio_base+0x1008000 + 0x03D4);
439     mem_out8(((2*1024 - 1)&0xf00)>>8, s3trio_base+0x1008000 + 0x03D5);
440
441     mem_out8(0x4d, s3trio_base+0x1008000 + 0x03D4);
442     mem_out8((2*1024 - 1) & 0xff, s3trio_base+0x1008000 + 0x03D5);
443
444     mem_out8(0x45, s3trio_base+0x1008000 + 0x03D4);
445     mem_in8(s3trio_base+0x1008000 + 0x03D4);
446
447     mem_out8(0x4a, s3trio_base+0x1008000 + 0x03D4);
448     mem_out8(0x80, s3trio_base+0x1008000 + 0x03D5);
449     mem_out8(0x80, s3trio_base+0x1008000 + 0x03D5);
450     mem_out8(0x80, s3trio_base+0x1008000 + 0x03D5);
451
452     mem_out8(0x4b, s3trio_base+0x1008000 + 0x03D4);
453     mem_out8(0x00, s3trio_base+0x1008000 + 0x03D5);
454     mem_out8(0x00, s3trio_base+0x1008000 + 0x03D5);
455     mem_out8(0x00, s3trio_base+0x1008000 + 0x03D5);
456
457     mem_out8(0x45, s3trio_base+0x1008000 + 0x03D4);
458     mem_out8(0, s3trio_base+0x1008000 + 0x03D5);
459
460     /* setup default color table */
461
462         for(i = 0; i < 16; i++) {
463                 int j = color_table[i];
464                 palette[i].red=default_red[j];
465                 palette[i].green=default_grn[j];
466                 palette[i].blue=default_blu[j];
467         }
468
469     s3trio_setcolreg(255, 56, 100, 160, 0, NULL /* not used */);
470     s3trio_setcolreg(254, 0, 0, 0, 0, NULL /* not used */);
471     memset((char *)s3trio_base, 0, 640*480);
472
473 #if 0
474     Trio_RectFill(0, 0, 90, 90, 7, 1);
475 #endif
476
477     fb_fix.visual = FB_VISUAL_PSEUDOCOLOR ;
478     fb_var.xoffset = fb_var.yoffset = 0;
479     fb_var.bits_per_pixel = 8;
480     fb_var.grayscale = 0;
481     fb_var.red.offset = fb_var.green.offset = fb_var.blue.offset = 0;
482     fb_var.red.length = fb_var.green.length = fb_var.blue.length = 8;
483     fb_var.red.msb_right = fb_var.green.msb_right = fb_var.blue.msb_right = 0;
484     fb_var.transp.offset = fb_var.transp.length = fb_var.transp.msb_right = 0;
485     fb_var.nonstd = 0;
486     fb_var.activate = 0;
487     fb_var.height = fb_var.width = -1;
488     fb_var.accel_flags = FB_ACCELF_TEXT;
489 #warning FIXME: always obey fb_var.accel_flags
490     fb_var.pixclock = 1;
491     fb_var.left_margin = fb_var.right_margin = 0;
492     fb_var.upper_margin = fb_var.lower_margin = 0;
493     fb_var.hsync_len = fb_var.vsync_len = 0;
494     fb_var.sync = 0;
495     fb_var.vmode = FB_VMODE_NONINTERLACED;
496
497     disp.var = fb_var;
498     disp.cmap.start = 0;
499     disp.cmap.len = 0;
500     disp.cmap.red = disp.cmap.green = disp.cmap.blue = disp.cmap.transp = NULL;
501     disp.visual = fb_fix.visual;
502     disp.type = fb_fix.type;
503     disp.type_aux = fb_fix.type_aux;
504     disp.ypanstep = 0;
505     disp.ywrapstep = 0;
506     disp.line_length = fb_fix.line_length;
507     disp.can_soft_blank = 1;
508     disp.inverse = 0;
509 #ifdef FBCON_HAS_CFB8
510     if (fb_var.accel_flags & FB_ACCELF_TEXT)
511         disp.dispsw = &fbcon_trio8;
512     else
513         disp.dispsw = &fbcon_cfb8;
514 #else
515     disp.dispsw = &fbcon_dummy;
516 #endif
517     disp.scrollmode = fb_var.accel_flags & FB_ACCELF_TEXT ? 0 : SCROLL_YREDRAW;
518
519     strcpy(fb_info.modename, "Trio64 ");
520     strncat(fb_info.modename, dp->full_name, sizeof(fb_info.modename));
521     fb_info.currcon = -1;
522     fb_info.fbops = &s3trio_ops;
523     fb_info.screen_base = s3trio_base;  
524 #if 0
525     fb_info.fbvar_num = 1;
526     fb_info.fbvar = &fb_var;
527 #endif
528     fb_info.disp = &disp;
529     fb_info.fontname[0] = '\0';
530     fb_info.changevar = NULL;
531     fb_info.switch_con = &s3triofbcon_switch;
532     fb_info.updatevar = &s3triofbcon_updatevar;
533 #if 0
534     fb_info.setcmap = &s3triofbcon_setcmap;
535 #endif
536
537     fb_info.flags = FBINFO_FLAG_DEFAULT;
538     if (register_framebuffer(&fb_info) < 0)
539         return;
540
541     printk("fb%d: S3 Trio frame buffer device on %s\n",
542            fb_info.node, dp->full_name);
543 }
544
545
546 static int s3triofbcon_switch(int con, struct fb_info *info)
547 {
548     /* Do we have to save the colormap? */
549     if (fb_display[info->currcon].cmap.len)
550         fb_get_cmap(&fb_display[info->currcon].cmap, 1, s3trio_getcolreg, info);
551
552     info->currcon = con;
553     /* Install new colormap */
554     do_install_cmap(con,info);
555     return 0;
556 }
557
558     /*
559      *  Update the `var' structure (called by fbcon.c)
560      */
561
562 static int s3triofbcon_updatevar(int con, struct fb_info *info)
563 {
564     /* Nothing */
565     return 0;
566 }
567
568     /*
569      *  Blank the display.
570      */
571
572 static int s3triofb_blank(int blank, struct fb_info *info)
573 {
574     unsigned char x;
575
576     mem_out8(0x1, s3trio_base+0x1008000 + 0x03c4);
577     x = mem_in8(s3trio_base+0x1008000 + 0x03c5);
578     mem_out8((x & (~0x20)) | (blank << 5), s3trio_base+0x1008000 + 0x03c5);
579     return 0;   
580 }
581
582     /*
583      *  Read a single color register and split it into
584      *  colors/transparent. Return != 0 for invalid regno.
585      */
586
587 static int s3trio_getcolreg(u_int regno, u_int *red, u_int *green, u_int *blue,
588                          u_int *transp, struct fb_info *info)
589 {
590     if (regno > 255)
591         return 1;
592     *red = (palette[regno].red << 8) | palette[regno].red;
593     *green = (palette[regno].green << 8) | palette[regno].green;
594     *blue = (palette[regno].blue << 8) | palette[regno].blue;
595     *transp = 0;
596     return 0;
597 }
598
599
600     /*
601      *  Set a single color register. Return != 0 for invalid regno.
602      */
603
604 static int s3trio_setcolreg(u_int regno, u_int red, u_int green, u_int blue,
605                             u_int transp, struct fb_info *info)
606 {
607     if (regno > 255)
608         return 1;
609
610     red >>= 8;
611     green >>= 8;
612     blue >>= 8;
613     palette[regno].red = red;
614     palette[regno].green = green;
615     palette[regno].blue = blue;
616
617     mem_out8(regno,s3trio_base+0x1008000 + 0x3c8);
618     mem_out8((red & 0xff) >> 2,s3trio_base+0x1008000 + 0x3c9);
619     mem_out8((green & 0xff) >> 2,s3trio_base+0x1008000 + 0x3c9);
620     mem_out8((blue & 0xff) >> 2,s3trio_base+0x1008000 + 0x3c9);
621
622     return 0;
623 }
624
625 static void Trio_WaitQueue(u_short fifo) {
626
627         u_short status;
628
629         do
630         {
631                 status = mem_in16(s3trio_base + 0x1000000 + 0x9AE8);
632         }  while (!(status & fifo));
633
634 }
635
636 static void Trio_WaitBlit(void) {
637
638         u_short status;
639
640         do
641         {
642                 status = mem_in16(s3trio_base + 0x1000000 + 0x9AE8);
643         }  while (status & 0x200);
644
645 }
646
647 static void Trio_BitBLT(u_short curx, u_short cury, u_short destx,
648                         u_short desty, u_short width, u_short height,
649                         u_short mode) {
650
651         u_short blitcmd = 0xc011;
652
653         /* Set drawing direction */
654         /* -Y, X maj, -X (default) */
655
656         if (curx > destx)
657                 blitcmd |= 0x0020;  /* Drawing direction +X */
658         else {
659                 curx  += (width - 1);
660                 destx += (width - 1);
661         }
662
663         if (cury > desty)
664                 blitcmd |= 0x0080;  /* Drawing direction +Y */
665         else {
666                 cury  += (height - 1);
667                 desty += (height - 1);
668         }
669
670         Trio_WaitQueue(0x0400);
671
672         outw(0xa000,  0xBEE8);
673         outw(0x60 | mode,  0xBAE8);
674
675         outw(curx,  0x86E8);
676         outw(cury,  0x82E8);
677
678         outw(destx,  0x8EE8);
679         outw(desty,  0x8AE8);
680
681         outw(height - 1,  0xBEE8);
682         outw(width - 1,  0x96E8);
683
684         outw(blitcmd,  0x9AE8);
685
686 }
687
688 static void Trio_RectFill(u_short x, u_short y, u_short width, u_short height,
689                           u_short mode, u_short color) {
690
691         u_short blitcmd = 0x40b1;
692
693         Trio_WaitQueue(0x0400);
694
695         outw(0xa000,  0xBEE8);
696         outw((0x20 | mode),  0xBAE8);
697         outw(0xe000,  0xBEE8);
698         outw(color,  0xA6E8);
699         outw(x,  0x86E8);
700         outw(y,  0x82E8);
701         outw((height - 1), 0xBEE8);
702         outw((width - 1), 0x96E8);
703         outw(blitcmd,  0x9AE8);
704
705 }
706
707
708 static void Trio_MoveCursor(u_short x, u_short y) {
709
710         mem_out8(0x39, s3trio_base + 0x1008000 + 0x3d4);
711         mem_out8(0xa0, s3trio_base + 0x1008000 + 0x3d5);
712
713         mem_out8(0x46, s3trio_base + 0x1008000 + 0x3d4);
714         mem_out8((x & 0x0700) >> 8, s3trio_base + 0x1008000 + 0x3d5);
715         mem_out8(0x47, s3trio_base + 0x1008000 + 0x3d4);
716         mem_out8(x & 0x00ff, s3trio_base + 0x1008000 + 0x3d5);
717
718         mem_out8(0x48, s3trio_base + 0x1008000 + 0x3d4);
719         mem_out8((y & 0x0700) >> 8, s3trio_base + 0x1008000 + 0x3d5);
720         mem_out8(0x49, s3trio_base + 0x1008000 + 0x3d4);
721         mem_out8(y & 0x00ff, s3trio_base + 0x1008000 + 0x3d5);
722
723 }
724
725
726     /*
727      *  Text console acceleration
728      */
729
730 #ifdef FBCON_HAS_CFB8
731 static void fbcon_trio8_bmove(struct display *p, int sy, int sx, int dy,
732                               int dx, int height, int width)
733 {
734     sx *= 8; dx *= 8; width *= 8;
735     Trio_BitBLT((u_short)sx, (u_short)(sy*fontheight(p)), (u_short)dx,
736                  (u_short)(dy*fontheight(p)), (u_short)width,
737                  (u_short)(height*fontheight(p)), (u_short)S3_NEW);
738 }
739
740 static void fbcon_trio8_clear(struct vc_data *conp, struct display *p, int sy,
741                               int sx, int height, int width)
742 {
743     unsigned char bg;
744
745     sx *= 8; width *= 8;
746     bg = attr_bgcol_ec(p,conp);
747     Trio_RectFill((u_short)sx,
748                    (u_short)(sy*fontheight(p)),
749                    (u_short)width,
750                    (u_short)(height*fontheight(p)),
751                    (u_short)S3_NEW,
752                    (u_short)bg);
753 }
754
755 static void fbcon_trio8_putc(struct vc_data *conp, struct display *p, int c,
756                              int yy, int xx)
757 {
758     Trio_WaitBlit();
759     fbcon_cfb8_putc(conp, p, c, yy, xx);
760 }
761
762 static void fbcon_trio8_putcs(struct vc_data *conp, struct display *p,
763                               const unsigned short *s, int count, int yy, int xx)
764 {
765     Trio_WaitBlit();
766     fbcon_cfb8_putcs(conp, p, s, count, yy, xx);
767 }
768
769 static void fbcon_trio8_revc(struct display *p, int xx, int yy)
770 {
771     Trio_WaitBlit();
772     fbcon_cfb8_revc(p, xx, yy);
773 }
774
775 static struct display_switch fbcon_trio8 = {
776    .setup =             fbcon_cfb8_setup,
777    .bmove =             fbcon_trio8_bmove,
778    .clear =             fbcon_trio8_clear,
779    .putc =              fbcon_trio8_putc,
780    .putcs =             fbcon_trio8_putcs,
781    .revc =              fbcon_trio8_revc,
782    .clear_margins =     fbcon_cfb8_clear_margins,
783    .fontwidthmask =     FONTWIDTH(8)
784 };
785 #endif
786
787 MODULE_LICENSE("GPL");