drivers/video/controlfb.c

   1 /*
   2  *  controlfb.c -- frame buffer device for the PowerMac 'control' display
   3  *
   4  *  Created 12 July 1998 by Dan Jacobowitz <dan@debian.org>
   5  *  Copyright (C) 1998 Dan Jacobowitz
   6  *  Copyright (C) 2001 Takashi Oe
   7  *
   8  *  Mmap code by Michel Lanners <mlan@cpu.lu>
   9  *
  10  *  Frame buffer structure from:
  11  *    drivers/video/chipsfb.c -- frame buffer device for
  12  *    Chips & Technologies 65550 chip.
  13  *
  14  *    Copyright (C) 1998 Paul Mackerras
  15  *
  16  *    This file is derived from the Powermac "chips" driver:
  17  *    Copyright (C) 1997 Fabio Riccardi.
  18  *    And from the frame buffer device for Open Firmware-initialized devices:
  19  *    Copyright (C) 1997 Geert Uytterhoeven.
  20  *
  21  *  Hardware information from:
  22  *    control.c: Console support for PowerMac "control" display adaptor.
  23  *    Copyright (C) 1996 Paul Mackerras
  24  *
  25  *  This file is subject to the terms and conditions of the GNU General Public
  26  *  License. See the file COPYING in the main directory of this archive for
  27  *  more details.
  28  */
  29
  30 #include <linux/config.h>
  31 #include <linux/module.h>
  32 #include <linux/kernel.h>
  33 #include <linux/errno.h>
  34 #include <linux/string.h>
  35 #include <linux/mm.h>
  36 #include <linux/tty.h>
  37 #include <linux/slab.h>
  38 #include <linux/vmalloc.h>
  39 #include <linux/delay.h>
  40 #include <linux/interrupt.h>
  41 #include <linux/fb.h>
  42 #include <linux/selection.h>
  43 #include <linux/init.h>
  44 #include <linux/pci.h>
  45 #include <linux/nvram.h>
  46 #ifdef CONFIG_FB_COMPAT_XPMAC
  47 #include <asm/vc_ioctl.h>
  48 #endif
  49 #include <linux/adb.h>
  50 #include <linux/cuda.h>
  51 #include <asm/io.h>
  52 #include <asm/prom.h>
  53 #include <asm/pgtable.h>
  54 #include <asm/btext.h>
  55
  56 #include <video/fbcon.h>
  57 #include <video/fbcon-cfb8.h>
  58 #include <video/fbcon-cfb16.h>
  59 #include <video/fbcon-cfb32.h>
  60 #include <video/macmodes.h>
  61
  62 #include "controlfb.h"
  63
  64 struct fb_par_control {
  65         int     vmode, cmode;
  66         int     xres, yres;
  67         int     vxres, vyres;
  68         int     xoffset, yoffset;
  69         int     pitch;
  70         struct control_regvals  regvals;
  71         unsigned long sync;
  72         unsigned char ctrl;
  73 };
  74
  75 #define DIRTY(z) ((x)->z != (y)->z)
  76 #define DIRTY_CMAP(z) (memcmp(&((x)->z), &((y)->z), sizeof((y)->z)))
  77 static inline int PAR_EQUAL(struct fb_par_control *x, struct fb_par_control *y)
  78 {
  79         int i, results;
  80
  81         results = 1;
  82         for (i = 0; i < 3; i++)
  83                 results &= !DIRTY(regvals.clock_params[i]);
  84         if (!results)
  85                 return 0;
  86         for (i = 0; i < 16; i++)
  87                 results &= !DIRTY(regvals.regs[i]);
  88         if (!results)
  89                 return 0;
  90         return (!DIRTY(cmode) && !DIRTY(xres) && !DIRTY(yres)
  91                 && !DIRTY(vxres) && !DIRTY(vyres));
  92 }
  93 static inline int VAR_MATCH(struct fb_var_screeninfo *x, struct fb_var_screeninfo *y)
  94 {
  95         return (!DIRTY(bits_per_pixel) && !DIRTY(xres)
  96                 && !DIRTY(yres) && !DIRTY(xres_virtual)
  97                 && !DIRTY(yres_virtual)
  98                 && !DIRTY_CMAP(red) && !DIRTY_CMAP(green) && !DIRTY_CMAP(blue));
  99 }
 100
 101 struct fb_info_control {
 102         struct fb_info                  info;
 103         struct display                  display;
 104         struct fb_par_control           par;
 105         struct {
 106                 __u8 red, green, blue;
 107         }                       palette[256];
 108
 109         struct cmap_regs        *cmap_regs;
 110         unsigned long           cmap_regs_phys;
 111
 112         struct control_regs     *control_regs;
 113         unsigned long           control_regs_phys;
 114         unsigned long           control_regs_size;
 115
 116         __u8                    *frame_buffer;
 117         unsigned long           frame_buffer_phys;
 118         unsigned long           fb_orig_base;
 119         unsigned long           fb_orig_size;
 120
 121         int                     control_use_bank2;
 122         unsigned long           total_vram;
 123         unsigned char           vram_attr;
 124         union {
 125 #ifdef FBCON_HAS_CFB16
 126                 u16 cfb16[16];
 127 #endif
 128 #ifdef FBCON_HAS_CFB32
 129                 u32 cfb32[16];
 130 #endif
 131         } fbcon_cmap;
 132 };
 133
 134 /* control register access macro */
 135 #define CNTRL_REG(INFO,REG) (&(((INFO)->control_regs->REG).r))
 136
 137
 138 /******************** Prototypes for exported functions ********************/
 139 /*
 140  * struct fb_ops
 141  */
 142 static int control_get_fix(struct fb_fix_screeninfo *fix, int con,
 143         struct fb_info *info);
 144 static int control_get_var(struct fb_var_screeninfo *var, int con,
 145         struct fb_info *info);
 146 static int control_set_var(struct fb_var_screeninfo *var, int con,
 147         struct fb_info *info);
 148 static int control_pan_display(struct fb_var_screeninfo *var, int con,
 149         struct fb_info *info);
 150 static int control_get_cmap(struct fb_cmap *cmap, int kspc, int con,
 151         struct fb_info *info);
 152 static int control_set_cmap(struct fb_cmap *cmap, int kspc, int con,
 153         struct fb_info *info);
 154 static int control_mmap(struct fb_info *info, struct file *file,
 155         struct vm_area_struct *vma);
 156
 157 /*
 158  * low level fbcon ops
 159  */
 160 static int controlfb_switch(int con, struct fb_info *info);
 161 static int controlfb_updatevar(int con, struct fb_info *info);
 162 static void controlfb_blank(int blank_mode, struct fb_info *info);
 163
 164 /*
 165  * low level cmap set/get ops
 166  */
 167 static int controlfb_getcolreg(u_int regno, u_int *red, u_int *green,
 168         u_int *blue, u_int *transp, struct fb_info *info);
 169 static int controlfb_setcolreg(u_int regno, u_int red, u_int green, u_int blue,
 170         u_int transp, struct fb_info *info);
 171
 172 /*
 173  * inititialization
 174  */
 175 int control_init(void);
 176 void control_setup(char *);
 177
 178 /*
 179  * low level fbcon revc ops
 180  */
 181 static void control_cfb16_revc(struct display *p, int xx, int yy);
 182 static void control_cfb32_revc(struct display *p, int xx, int yy);
 183
 184
 185 /******************** Prototypes for internal functions **********************/
 186
 187 static void do_install_cmap(struct display *disp, struct fb_info *info);
 188 static void set_control_clock(unsigned char *params);
 189 static int init_control(struct fb_info_control *p);
 190 static void control_set_hardware(struct fb_info_control *p,
 191         struct fb_par_control *par);
 192 static int control_of_init(struct device_node *dp);
 193 static void control_par_to_fix(struct fb_par_control *par,
 194         struct fb_fix_screeninfo *fix, struct fb_info_control *p);
 195 static void control_set_dispsw(struct display *disp, int cmode,
 196         struct fb_info_control *p);
 197 static void find_vram_size(struct fb_info_control *p);
 198 static int read_control_sense(struct fb_info_control *p);
 199 static int calc_clock_params(unsigned long clk, unsigned char *param);
 200 static int control_var_to_par(struct fb_var_screeninfo *var,
 201         struct fb_par_control *par, const struct fb_info *fb_info);
 202 static inline void control_par_to_var(struct fb_par_control *par,
 203         struct fb_var_screeninfo *var);
 204 static void control_par_to_fix(struct fb_par_control *par,
 205         struct fb_fix_screeninfo *fix, struct fb_info_control *p);
 206 static void control_par_to_display(struct fb_par_control *par,
 207         struct display *disp, struct fb_fix_screeninfo *fix,
 208         struct fb_info_control *p);
 209 static void control_set_dispsw(struct display *disp, int cmode,
 210         struct fb_info_control *p);
 211 static void control_init_info(struct fb_info *info, struct fb_info_control *p);
 212 static void control_cleanup(void);
 213
 214
 215 /************************** Internal variables *******************************/
 216
 217 static int currcon;
 218 static struct fb_info_control *control_fb;
 219
 220 static char fontname[40] __initdata = { 0 };
 221 static int default_vmode __initdata = VMODE_NVRAM;
 222 static int default_cmode __initdata = CMODE_NVRAM;
 223
 224
 225 static struct fb_ops controlfb_ops = {
 226         owner:          THIS_MODULE,
 227         fb_get_fix:     control_get_fix,
 228         fb_get_var:     control_get_var,
 229         fb_set_var:     control_set_var,
 230         fb_get_cmap:    control_get_cmap,
 231         fb_set_cmap:    control_set_cmap,
 232         fb_pan_display: control_pan_display,
 233         fb_mmap:        control_mmap,
 234 };
 235
 236
 237 /********************  The functions for controlfb_ops ********************/
 238
 239 #ifdef MODULE
 240 MODULE_LICENSE("GPL");
 241
 242 int init_module(void)
 243 {
 244         struct device_node *dp;
 245
 246         dp = find_devices("control");
 247         if (dp != 0 && !control_of_init(dp))
 248                 return 0;
 249
 250         return -ENXIO;
 251 }
 252
 253 void cleanup_module(void)
 254 {
 255         control_cleanup();
 256 }
 257 #endif
 258
 259 /*********** Providing our information to the user ************/
 260
 261 static int control_get_fix(struct fb_fix_screeninfo *fix, int con,
 262                          struct fb_info *info)
 263 {
 264         struct fb_info_control *p = (struct fb_info_control *) info;
 265
 266         if(con == -1) {
 267                 control_par_to_fix(&p->par, fix, p);
 268         } else {
 269                 struct fb_par_control par;
 270
 271                 control_var_to_par(&fb_display[con].var, &par, info);
 272                 control_par_to_fix(&par, fix, p);
 273         }
 274         return 0;
 275 }
 276
 277 static int control_get_var(struct fb_var_screeninfo *var, int con,
 278                          struct fb_info *info)
 279 {
 280         struct fb_info_control *p = (struct fb_info_control *) info;
 281
 282         if(con == -1) {
 283                 control_par_to_var(&p->par, var);
 284         } else {
 285                 *var = fb_display[con].var;
 286         }
 287         return 0;
 288 }
 289
 290
 291 /*
 292  * Sets everything according to var
 293  */
 294 static int control_set_var(struct fb_var_screeninfo *var, int con,
 295                          struct fb_info *info)
 296 {
 297         struct fb_info_control *p = (struct fb_info_control *) info;
 298         struct display *disp;
 299         struct fb_par_control par;
 300         int depthchange, err;
 301         int activate = var->activate;
 302
 303         if((err = control_var_to_par(var, &par, info))) {
 304                 if (con < 0)
 305                         printk (KERN_ERR "control_set_var: error calling"
 306                                          " control_var_to_par: %d.\n", err);
 307                 return err;
 308         }
 309
 310         control_par_to_var(&par, var);
 311
 312         if ((activate & FB_ACTIVATE_MASK) != FB_ACTIVATE_NOW)
 313                 return 0;
 314
 315         disp = (con >= 0) ? &fb_display[con] : info->disp;
 316
 317         depthchange = (disp->var.bits_per_pixel != var->bits_per_pixel);
 318         if(!VAR_MATCH(&disp->var, var)) {
 319                 struct fb_fix_screeninfo        fix;
 320                 control_par_to_fix(&par, &fix, p);
 321                 control_par_to_display(&par, disp, &fix, p);
 322                 if(info->changevar)
 323                         (*info->changevar)(con);
 324         } else
 325                 disp->var = *var;
 326
 327         if(con == currcon) {
 328                 control_set_hardware(p, &par);
 329                 if(depthchange) {
 330                         if((err = fb_alloc_cmap(&disp->cmap, 0, 0)))
 331                                 return err;
 332                         do_install_cmap(disp, info);
 333                 }
 334         }
 335
 336         return 0;
 337 }
 338
 339
 340 /*
 341  * Set screen start address according to var offset values
 342  */
 343 static inline void set_screen_start(int xoffset, int yoffset,
 344         struct fb_info_control *p)
 345 {
 346         struct fb_par_control *par = &p->par;
 347
 348         par->xoffset = xoffset;
 349         par->yoffset = yoffset;
 350         out_le32(CNTRL_REG(p,start_addr),
 351                  par->yoffset * par->pitch + (par->xoffset << par->cmode));
 352 }
 353
 354
 355 static int control_pan_display(struct fb_var_screeninfo *var, int con,
 356                              struct fb_info *info)
 357 {
 358         unsigned int xoffset, hstep;
 359         struct fb_info_control *p = (struct fb_info_control *)info;
 360         struct fb_par_control *par = &p->par;
 361
 362         /*
 363          * make sure start addr will be 32-byte aligned
 364          */
 365         hstep = 0x1f >> par->cmode;
 366         xoffset = (var->xoffset + hstep) & ~hstep;
 367
 368         if (xoffset+par->xres > par->vxres ||
 369             var->yoffset+par->yres > par->vyres)
 370                 return -EINVAL;
 371
 372         set_screen_start(xoffset, var->yoffset, p);
 373
 374         return 0;
 375 }
 376
 377 static int control_get_cmap(struct fb_cmap *cmap, int kspc, int con,
 378                           struct fb_info *info)
 379 {
 380         if (con == currcon)             /* current console? */
 381                 return fb_get_cmap(cmap, kspc, controlfb_getcolreg, info);
 382         if (fb_display[con].cmap.len)   /* non default colormap? */
 383                 fb_copy_cmap(&fb_display[con].cmap, cmap, kspc ? 0: 2);
 384         else {
 385                 int size = fb_display[con].var.bits_per_pixel == 16 ? 32 : 256;
 386                 fb_copy_cmap(fb_default_cmap(size), cmap, kspc ? 0 : 2);
 387         }
 388         return 0;
 389 }
 390
 391 static int control_set_cmap(struct fb_cmap *cmap, int kspc, int con,
 392                          struct fb_info *info)
 393 {
 394         struct display *disp = (con < 0)? info->disp: &fb_display[con];
 395         int err, size = disp->var.bits_per_pixel == 16 ? 32 : 256;
 396
 397         if (disp->cmap.len != size) {
 398                 err = fb_alloc_cmap(&disp->cmap, size, 0);
 399                 if (err)
 400                         return err;
 401         }
 402         if (con == currcon)
 403                 return fb_set_cmap(cmap, kspc, controlfb_setcolreg, info);
 404         fb_copy_cmap(cmap, &disp->cmap, kspc ? 0 : 1);
 405         return 0;
 406 }
 407
 408
 409 /*
 410  * Private mmap since we want to have a different caching on the framebuffer
 411  * for controlfb.
 412  * Note there's no locking in here; it's done in fb_mmap() in fbmem.c.
 413  */
 414 static int control_mmap(struct fb_info *info, struct file *file,
 415                        struct vm_area_struct *vma)
 416 {
 417        struct fb_ops *fb = info->fbops;
 418        struct fb_fix_screeninfo fix;
 419        struct fb_var_screeninfo var;
 420        unsigned long off, start;
 421        u32 len;
 422
 423        fb->fb_get_fix(&fix, PROC_CONSOLE(info), info);
 424        off = vma->vm_pgoff << PAGE_SHIFT;
 425
 426        /* frame buffer memory */
 427        start = fix.smem_start;
 428        len = PAGE_ALIGN((start & ~PAGE_MASK)+fix.smem_len);
 429        if (off >= len) {
 430                /* memory mapped io */
 431                off -= len;
 432                fb->fb_get_var(&var, PROC_CONSOLE(info), info);
 433                if (var.accel_flags)
 434                        return -EINVAL;
 435                start = fix.mmio_start;
 436                len = PAGE_ALIGN((start & ~PAGE_MASK)+fix.mmio_len);
 437                pgprot_val(vma->vm_page_prot) |= _PAGE_NO_CACHE|_PAGE_GUARDED;
 438        } else {
 439                /* framebuffer */
 440                pgprot_val(vma->vm_page_prot) |= _PAGE_WRITETHRU;
 441        }
 442        start &= PAGE_MASK;
 443        if ((vma->vm_end - vma->vm_start + off) > len)
 444                 return -EINVAL;
 445        off += start;
 446        vma->vm_pgoff = off >> PAGE_SHIFT;
 447        if (io_remap_page_range(vma->vm_start, off,
 448            vma->vm_end - vma->vm_start, vma->vm_page_prot))
 449                return -EAGAIN;
 450
 451        return 0;
 452 }
 453
 454
 455 /********************  End of controlfb_ops implementation  ******************/
 456
 457 /*
 458  * low level fbcon ops
 459  */
 460
 461 static int controlfb_switch(int con, struct fb_info *info)
 462 {
 463         struct fb_info_control  *p = (struct fb_info_control *)info;
 464         struct fb_par_control   par;
 465
 466         if (currcon >= 0 && fb_display[currcon].cmap.len)
 467                 fb_get_cmap(&fb_display[currcon].cmap, 1, controlfb_getcolreg,
 468                             info);
 469         currcon = con;
 470
 471         fb_display[con].var.activate = FB_ACTIVATE_NOW;
 472         control_var_to_par(&fb_display[con].var, &par, info);
 473         control_set_hardware(p, &par);
 474         control_set_dispsw(&fb_display[con], par.cmode, p);
 475         do_install_cmap(&fb_display[con], info);
 476
 477         return 1;
 478 }
 479
 480
 481 static int controlfb_updatevar(int con, struct fb_info *info)
 482 {
 483         struct fb_var_screeninfo *var = &fb_display[con].var;
 484         struct fb_info_control *p = (struct fb_info_control *) info;
 485
 486         set_screen_start(var->xoffset, var->yoffset, p);
 487
 488         return 0;
 489 }
 490
 491
 492 static void controlfb_blank(int blank_mode, struct fb_info *info)
 493 {
 494         struct fb_info_control *p = (struct fb_info_control *) info;
 495         unsigned ctrl;
 496
 497         ctrl = ld_le32(CNTRL_REG(p,ctrl));
 498         if (blank_mode > 0)
 499                 switch (blank_mode - 1) {
 500                 case VESA_VSYNC_SUSPEND:
 501                         ctrl &= ~3;
 502                         break;
 503                 case VESA_HSYNC_SUSPEND:
 504                         ctrl &= ~0x30;
 505                         break;
 506                 case VESA_POWERDOWN:
 507                         ctrl &= ~0x33;
 508                         /* fall through */
 509                 case VESA_NO_BLANKING:
 510                         ctrl |= 0x400;
 511                         break;
 512                 default:
 513                         break;
 514                 }
 515         else {
 516                 ctrl &= ~0x400;
 517                 ctrl |= 0x33;
 518         }
 519         out_le32(CNTRL_REG(p,ctrl), ctrl);
 520
 521         return;
 522 }
 523
 524
 525 /*
 526  * low level cmap set/get ops
 527  */
 528
 529 static int controlfb_getcolreg(u_int regno, u_int *red, u_int *green,
 530                              u_int *blue, u_int *transp, struct fb_info *info)
 531 {
 532         struct fb_info_control *p = (struct fb_info_control *) info;
 533
 534         if (regno > 255)
 535                 return 1;
 536         *red = (p->palette[regno].red<<8) | p->palette[regno].red;
 537         *green = (p->palette[regno].green<<8) | p->palette[regno].green;
 538         *blue = (p->palette[regno].blue<<8) | p->palette[regno].blue;
 539         *transp = 0;
 540         return 0;
 541 }
 542
 543 static int controlfb_setcolreg(u_int regno, u_int red, u_int green, u_int blue,
 544                              u_int transp, struct fb_info *info)
 545 {
 546         struct fb_info_control *p = (struct fb_info_control *) info;
 547         u_int i;
 548         __u8 r, g, b;
 549
 550         if (regno > 255)
 551                 return 1;
 552
 553         r = red >> 8;
 554         g = green >> 8;
 555         b = blue >> 8;
 556
 557         p->palette[regno].red = r;
 558         p->palette[regno].green = g;
 559         p->palette[regno].blue = b;
 560
 561         out_8(&p->cmap_regs->addr, regno);      /* tell clut what addr to fill  */
 562         out_8(&p->cmap_regs->lut, r);           /* send one color channel at    */
 563         out_8(&p->cmap_regs->lut, g);           /* a time...                    */
 564         out_8(&p->cmap_regs->lut, b);
 565
 566         if (regno < 16)
 567                 switch (p->par.cmode) {
 568 #ifdef FBCON_HAS_CFB16
 569                         case CMODE_16:
 570                                 p->fbcon_cmap.cfb16[regno] = (regno << 10) | (regno << 5) | regno;
 571                                 break;
 572 #endif
 573 #ifdef FBCON_HAS_CFB32
 574                         case CMODE_32:
 575                                 i = (regno << 8) | regno;
 576                                 p->fbcon_cmap.cfb32[regno] = (i << 16) | i;
 577                                 break;
 578 #endif
 579                 }
 580         return 0;
 581 }
 582
 583 static void do_install_cmap(struct display *disp, struct fb_info *info)
 584 {
 585         if (disp->cmap.len)
 586                 fb_set_cmap(&disp->cmap, 1, controlfb_setcolreg,
 587                             info);
 588         else {
 589                 int size = disp->var.bits_per_pixel == 16 ? 32 : 256;
 590                 fb_set_cmap(fb_default_cmap(size), 1, controlfb_setcolreg,
 591                             info);
 592         }
 593 }
 594
 595
 596 static void set_control_clock(unsigned char *params)
 597 {
 598 #ifdef CONFIG_ADB_CUDA
 599         struct adb_request req;
 600         int i;
 601
 602         for (i = 0; i < 3; ++i) {
 603                 cuda_request(&req, NULL, 5, CUDA_PACKET, CUDA_GET_SET_IIC,
 604                              0x50, i + 1, params[i]);
 605                 while (!req.complete)
 606                         cuda_poll();
 607         }
 608 #endif
 609 }
 610
 611
 612 /*
 613  * finish off the driver initialization and register
 614  */
 615 static int __init init_control(struct fb_info_control *p)
 616 {
 617         int full, sense, vmode, cmode, vyres;
 618         struct fb_var_screeninfo var;
 619
 620         printk(KERN_INFO "controlfb: ");
 621
 622         full = p->total_vram == 0x400000;
 623
 624 #ifdef CONFIG_NVRAM
 625         /* Try to pick a video mode out of NVRAM if we have one. */
 626         if (default_cmode == CMODE_NVRAM)
 627                 cmode = nvram_read_byte(NV_CMODE);
 628         if (default_vmode == VMODE_NVRAM) {
 629                 vmode = nvram_read_byte(NV_VMODE);
 630                 if (vmode < 1 || vmode > VMODE_MAX ||
 631                     control_mac_modes[vmode - 1].m[full] < cmode) {
 632                         sense = read_control_sense(p);
 633                         printk("Monitor sense value = 0x%x, ", sense);
 634                         vmode = mac_map_monitor_sense(sense);
 635                         if (control_mac_modes[vmode - 1].m[full] < cmode)
 636                                 vmode = VMODE_640_480_60;
 637                 }
 638         }
 639 #endif
 640
 641         /* If we didn't get something from NVRAM, pick a
 642          * sane default.
 643          */
 644         if (vmode <= 0 || vmode > VMODE_MAX)
 645                 vmode = VMODE_640_480_67;
 646         if (cmode < CMODE_8 || cmode > CMODE_32)
 647                 cmode = CMODE_8;
 648
 649         if (mac_vmode_to_var(vmode, cmode, &var) < 0) {
 650                 /* This shouldn't happen! */
 651                 printk("mac_vmode_to_var(%d, %d,) failed\n", vmode, cmode);
 652 try_again:
 653                 vmode = VMODE_640_480_60;
 654                 cmode = CMODE_8;
 655                 if (mac_vmode_to_var(vmode, cmode, &var) < 0) {
 656                         printk(KERN_ERR "controlfb: mac_vmode_to_var() failed\n");
 657                         return -ENXIO;
 658                 }
 659                 printk(KERN_INFO "controlfb: ");
 660         }
 661         printk("using video mode %d and color mode %d.\n", vmode, cmode);
 662
 663         vyres = (p->total_vram - CTRLFB_OFF) / (var.xres << cmode);
 664         if (vyres > var.yres)
 665                 var.yres_virtual = vyres;
 666
 667         control_init_info(&p->info, p);
 668         currcon = -1;
 669         var.activate = FB_ACTIVATE_NOW;
 670
 671         if (control_set_var(&var, -1, &p->info) < 0) {
 672                 if (vmode != VMODE_640_480_60 || cmode != CMODE_8)
 673                         goto try_again;
 674                 printk(KERN_ERR "controlfb: initilization failed\n");
 675                 return -ENXIO;
 676         }
 677
 678         p->info.flags = FBINFO_FLAG_DEFAULT;
 679         if (register_framebuffer(&p->info) < 0)
 680                 return -ENXIO;
 681
 682         printk(KERN_INFO "fb%d: control display adapter\n", GET_FB_IDX(p->info.node));
 683
 684         return 0;
 685 }
 686
 687 #define RADACAL_WRITE(a,d) \
 688         out_8(&p->cmap_regs->addr, (a)); \
 689         out_8(&p->cmap_regs->dat,   (d))
 690
 691 /* Now how about actually saying, Make it so! */
 692 /* Some things in here probably don't need to be done each time. */
 693 static void control_set_hardware(struct fb_info_control *p, struct fb_par_control *par)
 694 {
 695         struct control_regvals  *r;
 696         volatile struct preg    *rp;
 697         int                     i, cmode;
 698
 699         if (PAR_EQUAL(&p->par, par)) {
 700                 /*
 701                  * check if only xoffset or yoffset differs.
 702                  * this prevents flickers in typical VT switch case.
 703                  */
 704                 if (p->par.xoffset != par->xoffset ||
 705                     p->par.yoffset != par->yoffset)
 706                         set_screen_start(par->xoffset, par->yoffset, p);
 707
 708                 return;
 709         }
 710
 711         p->par = *par;
 712         cmode = p->par.cmode;
 713         r = &par->regvals;
 714
 715         /* Turn off display */
 716         out_le32(CNTRL_REG(p,ctrl), 0x400 | par->ctrl);
 717
 718         set_control_clock(r->clock_params);
 719
 720         RADACAL_WRITE(0x20, r->radacal_ctrl);
 721         RADACAL_WRITE(0x21, p->control_use_bank2 ? 0 : 1);
 722         RADACAL_WRITE(0x10, 0);
 723         RADACAL_WRITE(0x11, 0);
 724
 725         rp = &p->control_regs->vswin;
 726         for (i = 0; i < 16; ++i, ++rp)
 727                 out_le32(&rp->r, r->regs[i]);
 728
 729         out_le32(CNTRL_REG(p,pitch), par->pitch);
 730         out_le32(CNTRL_REG(p,mode), r->mode);
 731         out_le32(CNTRL_REG(p,vram_attr), p->vram_attr);
 732         out_le32(CNTRL_REG(p,start_addr), par->yoffset * par->pitch
 733                  + (par->xoffset << cmode));
 734         out_le32(CNTRL_REG(p,rfrcnt), 0x1e5);
 735         out_le32(CNTRL_REG(p,intr_ena), 0);
 736
 737         /* Turn on display */
 738         out_le32(CNTRL_REG(p,ctrl), par->ctrl);
 739
 740 #ifdef CONFIG_FB_COMPAT_XPMAC
 741         /* And let the world know the truth. */
 742         if (!console_fb_info || console_fb_info == &p->info) {
 743                 display_info.height = p->par.yres;
 744                 display_info.width = p->par.xres;
 745                 display_info.depth = (cmode == CMODE_32) ? 32 :
 746                         ((cmode == CMODE_16) ? 16 : 8);
 747                 display_info.pitch = p->par.pitch;
 748                 display_info.mode = p->par.vmode;
 749                 strncpy(display_info.name, "control",
 750                         sizeof(display_info.name));
 751                 display_info.fb_address = p->frame_buffer_phys + CTRLFB_OFF;
 752                 display_info.cmap_adr_address = p->cmap_regs_phys;
 753                 display_info.cmap_data_address = p->cmap_regs_phys + 0x30;
 754                 display_info.disp_reg_address = p->control_regs_phys;
 755                 console_fb_info = &p->info;
 756         }
 757 #endif /* CONFIG_FB_COMPAT_XPMAC */
 758 #ifdef CONFIG_BOOTX_TEXT
 759         btext_update_display(p->frame_buffer_phys + CTRLFB_OFF,
 760                              p->par.xres, p->par.yres,
 761                              (cmode == CMODE_32? 32: cmode == CMODE_16? 16: 8),
 762                              p->par.pitch);
 763 #endif /* CONFIG_BOOTX_TEXT */
 764 }
 765
 766
 767 /*
 768  * Called from fbmem.c for probing & intializing
 769  */
 770 int __init control_init(void)
 771 {
 772         struct device_node *dp;
 773
 774         dp = find_devices("control");
 775         if (dp != 0 && !control_of_init(dp))
 776                 return 0;
 777
 778         return -ENXIO;
 779 }
 780
 781
 782 /* Work out which banks of VRAM we have installed. */
 783 /* danj: I guess the card just ignores writes to nonexistant VRAM... */
 784
 785 static void __init find_vram_size(struct fb_info_control *p)
 786 {
 787         int bank1, bank2;
 788
 789         /*
 790          * Set VRAM in 2MB (bank 1) mode
 791          * VRAM Bank 2 will be accessible through offset 0x600000 if present
 792          * and VRAM Bank 1 will not respond at that offset even if present
 793          */
 794         out_le32(CNTRL_REG(p,vram_attr), 0x31);
 795
 796         out_8(&p->frame_buffer[0x600000], 0xb3);
 797         out_8(&p->frame_buffer[0x600001], 0x71);
 798         asm volatile("eieio; dcbf 0,%0" : : "r" (&p->frame_buffer[0x600000])
 799                                         : "memory" );
 800         mb();
 801         asm volatile("eieio; dcbi 0,%0" : : "r" (&p->frame_buffer[0x600000])
 802                                         : "memory" );
 803         mb();
 804
 805         bank2 = (in_8(&p->frame_buffer[0x600000]) == 0xb3)
 806                 && (in_8(&p->frame_buffer[0x600001]) == 0x71);
 807
 808         /*
 809          * Set VRAM in 2MB (bank 2) mode
 810          * VRAM Bank 1 will be accessible through offset 0x000000 if present
 811          * and VRAM Bank 2 will not respond at that offset even if present
 812          */
 813         out_le32(CNTRL_REG(p,vram_attr), 0x39);
 814
 815         out_8(&p->frame_buffer[0], 0x5a);
 816         out_8(&p->frame_buffer[1], 0xc7);
 817         asm volatile("eieio; dcbf 0,%0" : : "r" (&p->frame_buffer[0])
 818                                         : "memory" );
 819         mb();
 820         asm volatile("eieio; dcbi 0,%0" : : "r" (&p->frame_buffer[0])
 821                                         : "memory" );
 822         mb();
 823
 824         bank1 = (in_8(&p->frame_buffer[0]) == 0x5a)
 825                 && (in_8(&p->frame_buffer[1]) == 0xc7);
 826
 827         if (bank2) {
 828                 if (!bank1) {
 829                         /*
 830                          * vram bank 2 only
 831                          */
 832                         p->control_use_bank2 = 1;
 833                         p->vram_attr = 0x39;
 834                         p->frame_buffer += 0x600000;
 835                         p->frame_buffer_phys += 0x600000;
 836                 } else {
 837                         /*
 838                          * 4 MB vram
 839                          */
 840                         p->vram_attr = 0x51;
 841                 }
 842         } else {
 843                 /*
 844                  * vram bank 1 only
 845                  */
 846                 p->vram_attr = 0x31;
 847         }
 848
 849         p->total_vram = (bank1 + bank2) * 0x200000;
 850
 851         printk(KERN_INFO "controlfb: VRAM Total = %dMB "
 852                         "(%dMB @ bank 1, %dMB @ bank 2)\n",
 853                         (bank1 + bank2) << 1, bank1 << 1, bank2 << 1);
 854 }
 855
 856
 857 /*
 858  * find "control" and initialize
 859  */
 860 static int __init control_of_init(struct device_node *dp)
 861 {
 862         struct fb_info_control  *p;
 863         unsigned long           addr;
 864         int                     i;
 865
 866         if (control_fb) {
 867                 printk(KERN_ERR "controlfb: only one control is supported\n");
 868                 return -ENXIO;
 869         }
 870         if(dp->n_addrs != 2) {
 871                 printk(KERN_ERR "expecting 2 address for control (got %d)", dp->n_addrs);
 872                 return -ENXIO;
 873         }
 874         p = kmalloc(sizeof(*p), GFP_ATOMIC);
 875         if (p == 0)
 876                 return -ENXIO;
 877         control_fb = p; /* save it for cleanups */
 878         memset(p, 0, sizeof(*p));
 879
 880         /* Map in frame buffer and registers */
 881         for (i = 0; i < dp->n_addrs; ++i) {
 882                 addr = dp->addrs[i].address;
 883                 if (dp->addrs[i].size >= 0x800000) {
 884                         p->fb_orig_base = addr;
 885                         p->fb_orig_size = dp->addrs[i].size;
 886                         /* use the big-endian aperture (??) */
 887                         p->frame_buffer_phys = addr + 0x800000;
 888                 } else {
 889                         p->control_regs_phys = addr;
 890                         p->control_regs_size = dp->addrs[i].size;
 891                 }
 892         }
 893
 894         if (!p->fb_orig_base ||
 895             !request_mem_region(p->fb_orig_base,p->fb_orig_size,"controlfb")) {
 896                 p->fb_orig_base = 0;
 897                 goto error_out;
 898         }
 899         /* map at most 8MB for the frame buffer */
 900         p->frame_buffer = __ioremap(p->frame_buffer_phys, 0x800000,
 901                                     _PAGE_WRITETHRU);
 902
 903         if (!p->control_regs_phys ||
 904             !request_mem_region(p->control_regs_phys, p->control_regs_size,
 905             "controlfb regs")) {
 906                 p->control_regs_phys = 0;
 907                 goto error_out;
 908         }
 909         p->control_regs = ioremap(p->control_regs_phys, p->control_regs_size);
 910
 911         p->cmap_regs_phys = 0xf301b000;  /* XXX not in prom? */
 912         if (!request_mem_region(p->cmap_regs_phys, 0x1000, "controlfb cmap")) {
 913                 p->cmap_regs_phys = 0;
 914                 goto error_out;
 915         }
 916         p->cmap_regs = ioremap(p->cmap_regs_phys, 0x1000);
 917
 918         if (!p->cmap_regs || !p->control_regs || !p->frame_buffer)
 919                 goto error_out;
 920
 921         find_vram_size(p);
 922         if (!p->total_vram)
 923                 goto error_out;
 924
 925         if (init_control(p) < 0)
 926                 goto error_out;
 927
 928         return 0;
 929
 930 error_out:
 931         control_cleanup();
 932         return -ENXIO;
 933 }
 934
 935 /*
 936  * Get the monitor sense value.
 937  * Note that this can be called before calibrate_delay,
 938  * so we can't use udelay.
 939  */
 940 static int read_control_sense(struct fb_info_control *p)
 941 {
 942         int sense;
 943
 944         out_le32(CNTRL_REG(p,mon_sense), 7);    /* drive all lines high */
 945         __delay(200);
 946         out_le32(CNTRL_REG(p,mon_sense), 077);  /* turn off drivers */
 947         __delay(2000);
 948         sense = (in_le32(CNTRL_REG(p,mon_sense)) & 0x1c0) << 2;
 949
 950         /* drive each sense line low in turn and collect the other 2 */
 951         out_le32(CNTRL_REG(p,mon_sense), 033);  /* drive A low */
 952         __delay(2000);
 953         sense |= (in_le32(CNTRL_REG(p,mon_sense)) & 0xc0) >> 2;
 954         out_le32(CNTRL_REG(p,mon_sense), 055);  /* drive B low */
 955         __delay(2000);
 956         sense |= ((in_le32(CNTRL_REG(p,mon_sense)) & 0x100) >> 5)
 957                 | ((in_le32(CNTRL_REG(p,mon_sense)) & 0x40) >> 4);
 958         out_le32(CNTRL_REG(p,mon_sense), 066);  /* drive C low */
 959         __delay(2000);
 960         sense |= (in_le32(CNTRL_REG(p,mon_sense)) & 0x180) >> 7;
 961
 962         out_le32(CNTRL_REG(p,mon_sense), 077);  /* turn off drivers */
 963
 964         return sense;
 965 }
 966
 967 /**********************  Various translation functions  **********************/
 968
 969 #define CONTROL_PIXCLOCK_BASE   256016
 970 #define CONTROL_PIXCLOCK_MIN    5000    /* ~ 200 MHz dot clock */
 971
 972 /*
 973  * calculate the clock paramaters to be sent to CUDA according to given
 974  * pixclock in pico second.
 975  */
 976 static int calc_clock_params(unsigned long clk, unsigned char *param)
 977 {
 978         unsigned long p0, p1, p2, k, l, m, n, min;
 979
 980         if (clk > (CONTROL_PIXCLOCK_BASE << 3))
 981                 return 1;
 982
 983         p2 = ((clk << 4) < CONTROL_PIXCLOCK_BASE)? 3: 2;
 984         l = clk << p2;
 985         p0 = 0;
 986         p1 = 0;
 987         for (k = 1, min = l; k < 32; k++) {
 988                 unsigned long rem;
 989
 990                 m = CONTROL_PIXCLOCK_BASE * k;
 991                 n = m / l;
 992                 rem = m % l;
 993                 if (n && (n < 128) && rem < min) {
 994                         p0 = k;
 995                         p1 = n;
 996                         min = rem;
 997                 }
 998         }
 999         if (!p0 || !p1)
1000                 return 1;
1001
1002         param[0] = p0;
1003         param[1] = p1;
1004         param[2] = p2;
1005
1006         return 0;
1007 }
1008
1009
1010 /*
1011  * This routine takes a user-supplied var, and picks the best vmode/cmode
1012  * from it.
1013  */
1014
1015 static int control_var_to_par(struct fb_var_screeninfo *var,
1016         struct fb_par_control *par, const struct fb_info *fb_info)
1017 {
1018         int cmode, piped_diff, hstep;
1019         unsigned hperiod, hssync, hsblank, hesync, heblank, piped, heq, hlfln,
1020                  hserr, vperiod, vssync, vesync, veblank, vsblank, vswin, vewin;
1021         unsigned long pixclock;
1022         struct fb_info_control *p = (struct fb_info_control *) fb_info;
1023         struct control_regvals *r = &par->regvals;
1024
1025         switch (var->bits_per_pixel) {
1026         case 8:
1027                 par->cmode = CMODE_8;
1028                 if (p->total_vram > 0x200000) {
1029                         r->mode = 3;
1030                         r->radacal_ctrl = 0x20;
1031                         piped_diff = 13;
1032                 } else {
1033                         r->mode = 2;
1034                         r->radacal_ctrl = 0x10;
1035                         piped_diff = 9;
1036                 }
1037                 break;
1038         case 15:
1039         case 16:
1040                 par->cmode = CMODE_16;
1041                 if (p->total_vram > 0x200000) {
1042                         r->mode = 2;
1043                         r->radacal_ctrl = 0x24;
1044                         piped_diff = 5;
1045                 } else {
1046                         r->mode = 1;
1047                         r->radacal_ctrl = 0x14;
1048                         piped_diff = 3;
1049                 }
1050                 break;
1051         case 32:
1052                 par->cmode = CMODE_32;
1053                 if (p->total_vram > 0x200000) {
1054                         r->mode = 1;
1055                         r->radacal_ctrl = 0x28;
1056                 } else {
1057                         r->mode = 0;
1058                         r->radacal_ctrl = 0x18;
1059                 }
1060                 piped_diff = 1;
1061                 break;
1062         default:
1063                 return -EINVAL;
1064         }
1065
1066         /*
1067          * adjust xres and vxres so that the corresponding memory widths are
1068          * 32-byte aligned
1069          */
1070         hstep = 31 >> par->cmode;
1071         par->xres = (var->xres + hstep) & ~hstep;
1072         par->vxres = (var->xres_virtual + hstep) & ~hstep;
1073         par->xoffset = (var->xoffset + hstep) & ~hstep;
1074         if (par->vxres < par->xres)
1075                 par->vxres = par->xres;
1076         par->pitch = par->vxres << par->cmode;
1077
1078         par->yres = var->yres;
1079         par->vyres = var->yres_virtual;
1080         par->yoffset = var->yoffset;
1081         if (par->vyres < par->yres)
1082                 par->vyres = par->yres;
1083
1084         par->sync = var->sync;
1085
1086         if (par->pitch * par->vyres + CTRLFB_OFF > p->total_vram)
1087                 return -EINVAL;
1088
1089         if (par->xoffset + par->xres > par->vxres)
1090                 par->xoffset = par->vxres - par->xres;
1091         if (par->yoffset + par->yres > par->vyres)
1092                 par->yoffset = par->vyres - par->yres;
1093
1094         pixclock = (var->pixclock < CONTROL_PIXCLOCK_MIN)? CONTROL_PIXCLOCK_MIN:
1095                    var->pixclock;
1096         if (calc_clock_params(pixclock, r->clock_params))
1097                 return -EINVAL;
1098
1099         hperiod = ((var->left_margin + par->xres + var->right_margin
1100                     + var->hsync_len) >> 1) - 2;
1101         hssync = hperiod + 1;
1102         hsblank = hssync - (var->right_margin >> 1);
1103         hesync = (var->hsync_len >> 1) - 1;
1104         heblank = (var->left_margin >> 1) + hesync;
1105         piped = heblank - piped_diff;
1106         heq = var->hsync_len >> 2;
1107         hlfln = (hperiod+2) >> 1;
1108         hserr = hssync-hesync;
1109         vperiod = (var->vsync_len + var->lower_margin + par->yres
1110                    + var->upper_margin) << 1;
1111         vssync = vperiod - 2;
1112         vesync = (var->vsync_len << 1) - vperiod + vssync;
1113         veblank = (var->upper_margin << 1) + vesync;
1114         vsblank = vssync - (var->lower_margin << 1);
1115         vswin = (vsblank+vssync) >> 1;
1116         vewin = (vesync+veblank) >> 1;
1117
1118         r->regs[0] = vswin;
1119         r->regs[1] = vsblank;
1120         r->regs[2] = veblank;
1121         r->regs[3] = vewin;
1122         r->regs[4] = vesync;
1123         r->regs[5] = vssync;
1124         r->regs[6] = vperiod;
1125         r->regs[7] = piped;
1126         r->regs[8] = hperiod;
1127         r->regs[9] = hsblank;
1128         r->regs[10] = heblank;
1129         r->regs[11] = hesync;
1130         r->regs[12] = hssync;
1131         r->regs[13] = heq;
1132         r->regs[14] = hlfln;
1133         r->regs[15] = hserr;
1134
1135         if (par->xres >= 1280 && par->cmode >= CMODE_16)
1136                 par->ctrl = 0x7f;
1137         else
1138                 par->ctrl = 0x3b;
1139
1140         if (mac_var_to_vmode(var, &par->vmode, &cmode))
1141                 par->vmode = 0;
1142
1143         return 0;
1144 }
1145
1146
1147 /*
1148  * Convert hardware data in par to an fb_var_screeninfo
1149  */
1150
1151 static void control_par_to_var(struct fb_par_control *par, struct fb_var_screeninfo *var)
1152 {
1153         struct control_regints *rv;
1154
1155         rv = (struct control_regints *) par->regvals.regs;
1156
1157         memset(var, 0, sizeof(*var));
1158         var->xres = par->xres;
1159         var->yres = par->yres;
1160         var->xres_virtual = par->vxres;
1161         var->yres_virtual = par->vyres;
1162         var->xoffset = par->xoffset;
1163         var->yoffset = par->yoffset;
1164
1165         switch(par->cmode) {
1166         default:
1167         case CMODE_8:
1168                 var->bits_per_pixel = 8;
1169                 var->red.length = 8;
1170                 var->green.length = 8;
1171                 var->blue.length = 8;
1172                 break;
1173         case CMODE_16:  /* RGB 555 */
1174                 var->bits_per_pixel = 16;
1175                 var->red.offset = 10;
1176                 var->red.length = 5;
1177                 var->green.offset = 5;
1178                 var->green.length = 5;
1179                 var->blue.length = 5;
1180                 break;
1181         case CMODE_32:  /* RGB 888 */
1182                 var->bits_per_pixel = 32;
1183                 var->red.offset = 16;
1184                 var->red.length = 8;
1185                 var->green.offset = 8;
1186                 var->green.length = 8;
1187                 var->blue.length = 8;
1188                 var->transp.offset = 24;
1189                 var->transp.length = 8;
1190                 break;
1191         }
1192         var->height = -1;
1193         var->width = -1;
1194         var->vmode = FB_VMODE_NONINTERLACED;
1195
1196         var->left_margin = (rv->heblank - rv->hesync) << 1;
1197         var->right_margin = (rv->hssync - rv->hsblank) << 1;
1198         var->hsync_len = (rv->hperiod + 2 - rv->hssync + rv->hesync) << 1;
1199
1200         var->upper_margin = (rv->veblank - rv->vesync) >> 1;
1201         var->lower_margin = (rv->vssync - rv->vsblank) >> 1;
1202         var->vsync_len = (rv->vperiod - rv->vssync + rv->vesync) >> 1;
1203
1204         var->sync = par->sync;
1205
1206         /*
1207          * 10^12 * clock_params[0] / (3906400 * clock_params[1]
1208          *                            * 2^clock_params[2])
1209          * (10^12 * clock_params[0] / (3906400 * clock_params[1]))
1210          * >> clock_params[2]
1211          */
1212         /* (255990.17 * clock_params[0] / clock_params[1]) >> clock_params[2] */
1213         var->pixclock = CONTROL_PIXCLOCK_BASE * par->regvals.clock_params[0];
1214         var->pixclock /= par->regvals.clock_params[1];
1215         var->pixclock >>= par->regvals.clock_params[2];
1216 }
1217
1218
1219 /*
1220  * init fix according to given par
1221  */
1222 static void control_par_to_fix(struct fb_par_control *par, struct fb_fix_screeninfo *fix,
1223         struct fb_info_control *p)
1224 {
1225         memset(fix, 0, sizeof(*fix));
1226         strcpy(fix->id, "control");
1227         fix->mmio_start = p->control_regs_phys;
1228         fix->mmio_len = sizeof(struct control_regs);
1229         fix->type = FB_TYPE_PACKED_PIXELS;
1230
1231         fix->xpanstep = 32 >> par->cmode;
1232         fix->ypanstep = 1;
1233
1234         fix->smem_start = p->frame_buffer_phys + CTRLFB_OFF;
1235         fix->smem_len = p->total_vram - CTRLFB_OFF;
1236         fix->visual = (par->cmode == CMODE_8) ?
1237                 FB_VISUAL_PSEUDOCOLOR : FB_VISUAL_DIRECTCOLOR;
1238         fix->line_length = par->pitch;
1239 }
1240
1241 /*
1242  * initialize a portion of struct display which low level driver is responsible
1243  * for.
1244  */
1245 static void control_par_to_display(struct fb_par_control *par,
1246   struct display *disp, struct fb_fix_screeninfo *fix, struct fb_info_control *p)
1247 {
1248         disp->type = fix->type;
1249         disp->can_soft_blank = 1;
1250         disp->scrollmode = SCROLL_YNOMOVE | SCROLL_YNOPARTIAL;
1251         disp->ypanstep = fix->ypanstep;
1252         disp->ywrapstep = fix->ywrapstep;
1253
1254         control_par_to_var(par, &disp->var);
1255         disp->screen_base = (char *) p->frame_buffer + CTRLFB_OFF;
1256         disp->visual = fix->visual;
1257         disp->line_length = fix->line_length;
1258         control_set_dispsw(disp, par->cmode, p);
1259 }
1260
1261
1262 /*
1263  * our own _revc() routines since generic routines don't work for DIRECT Color
1264  * devices like control
1265  */
1266 static void control_cfb16_revc(struct display *p, int xx, int yy)
1267 {
1268     u8 *dest;
1269     int bytes = p->next_line, rows;
1270
1271     dest = p->screen_base + yy * fontheight(p) * bytes + xx * fontwidth(p)*2;
1272     for (rows = fontheight(p); rows--; dest += bytes) {
1273        switch (fontwidth(p)) {
1274        case 16:
1275            ((u32 *)dest)[6] ^= 0x3def3def; ((u32 *)dest)[7] ^= 0x3def3def;
1276            /* FALL THROUGH */
1277        case 12:
1278            ((u32 *)dest)[4] ^= 0x3def3def; ((u32 *)dest)[5] ^= 0x3def3def;
1279            /* FALL THROUGH */
1280        case 8:
1281            ((u32 *)dest)[2] ^= 0x3def3def; ((u32 *)dest)[3] ^= 0x3def3def;
1282            /* FALL THROUGH */
1283        case 4:
1284            ((u32 *)dest)[0] ^= 0x3def3def; ((u32 *)dest)[1] ^= 0x3def3def;
1285        }
1286     }
1287 }
1288
1289 static void control_cfb32_revc(struct display *p, int xx, int yy)
1290 {
1291     u8 *dest;
1292     int bytes = p->next_line, rows;
1293
1294     dest = p->screen_base + yy * fontheight(p) * bytes + xx * fontwidth(p) * 4;
1295     for (rows = fontheight(p); rows--; dest += bytes) {
1296        switch (fontwidth(p)) {
1297        case 16:
1298            ((u32 *)dest)[12] ^= 0x0f0f0f0f; ((u32 *)dest)[13] ^= 0x0f0f0f0f;
1299            ((u32 *)dest)[14] ^= 0x0f0f0f0f; ((u32 *)dest)[15] ^= 0x0f0f0f0f;
1300            /* FALL THROUGH */
1301        case 12:
1302            ((u32 *)dest)[8] ^= 0x0f0f0f0f; ((u32 *)dest)[9] ^= 0x0f0f0f0f;
1303            ((u32 *)dest)[10] ^= 0x0f0f0f0f; ((u32 *)dest)[11] ^= 0x0f0f0f0f;
1304            /* FALL THROUGH */
1305        case 8:
1306            ((u32 *)dest)[4] ^= 0x0f0f0f0f; ((u32 *)dest)[5] ^= 0x0f0f0f0f;
1307            ((u32 *)dest)[6] ^= 0x0f0f0f0f; ((u32 *)dest)[7] ^= 0x0f0f0f0f;
1308            /* FALL THROUGH */
1309        case 4:
1310            ((u32 *)dest)[0] ^= 0x0f0f0f0f; ((u32 *)dest)[1] ^= 0x0f0f0f0f;
1311            ((u32 *)dest)[2] ^= 0x0f0f0f0f; ((u32 *)dest)[3] ^= 0x0f0f0f0f;
1312            /* FALL THROUGH */
1313        }
1314     }
1315 }
1316
1317 static struct display_switch control_cfb16 = {
1318     setup:              fbcon_cfb16_setup,
1319     bmove:              fbcon_cfb16_bmove,
1320     clear:              fbcon_cfb16_clear,
1321     putc:               fbcon_cfb16_putc,
1322     putcs:              fbcon_cfb16_putcs,
1323     revc:               control_cfb16_revc,
1324     clear_margins:      fbcon_cfb16_clear_margins,
1325     fontwidthmask:      FONTWIDTH(4)|FONTWIDTH(8)|FONTWIDTH(12)|FONTWIDTH(16)
1326 };
1327
1328 static struct display_switch control_cfb32 = {
1329     setup:              fbcon_cfb32_setup,
1330     bmove:              fbcon_cfb32_bmove,
1331     clear:              fbcon_cfb32_clear,
1332     putc:               fbcon_cfb32_putc,
1333     putcs:              fbcon_cfb32_putcs,
1334     revc:               control_cfb32_revc,
1335     clear_margins:      fbcon_cfb32_clear_margins,
1336     fontwidthmask:      FONTWIDTH(4)|FONTWIDTH(8)|FONTWIDTH(12)|FONTWIDTH(16)
1337 };
1338
1339
1340 /*
1341  * Set struct dispsw according to given cmode
1342  */
1343 static void control_set_dispsw(struct display *disp, int cmode, struct fb_info_control *p)
1344 {
1345         switch (cmode) {
1346 #ifdef FBCON_HAS_CFB8
1347                 case CMODE_8:
1348                         disp->dispsw = &fbcon_cfb8;
1349                         break;
1350 #endif
1351 #ifdef FBCON_HAS_CFB16
1352                 case CMODE_16:
1353                         disp->dispsw = &control_cfb16;
1354                         disp->dispsw_data = p->fbcon_cmap.cfb16;
1355                         break;
1356 #endif
1357 #ifdef FBCON_HAS_CFB32
1358                 case CMODE_32:
1359                         disp->dispsw = &control_cfb32;
1360                         disp->dispsw_data = p->fbcon_cmap.cfb32;
1361                         break;
1362 #endif
1363                 default:
1364                         disp->dispsw = &fbcon_dummy;
1365                         break;
1366         }
1367 }
1368
1369
1370 /*
1371  * Set misc info vars for this driver
1372  */
1373 static void __init control_init_info(struct fb_info *info, struct fb_info_control *p)
1374 {
1375         strcpy(info->modename, "control");
1376         info->node = -1;        /* ??? danj */
1377         info->fbops = &controlfb_ops;
1378         info->disp = &p->display;
1379         strcpy(info->fontname, fontname);
1380         info->changevar = NULL;
1381         info->switch_con = &controlfb_switch;
1382         info->updatevar = &controlfb_updatevar;
1383         info->blank = &controlfb_blank;
1384 }
1385
1386
1387 static void control_cleanup(void)
1388 {
1389         struct fb_info_control  *p = control_fb;
1390
1391         if (!p)
1392                 return;
1393
1394         if (p->cmap_regs)
1395                 iounmap(p->cmap_regs);
1396         if (p->control_regs)
1397                 iounmap(p->control_regs);
1398         if (p->frame_buffer) {
1399                 if (p->control_use_bank2)
1400                         p->frame_buffer -= 0x600000;
1401                 iounmap(p->frame_buffer);
1402         }
1403         if (p->cmap_regs_phys)
1404                 release_mem_region(p->cmap_regs_phys, 0x1000);
1405         if (p->control_regs_phys)
1406                 release_mem_region(p->control_regs_phys, p->control_regs_size);
1407         if (p->fb_orig_base)
1408                 release_mem_region(p->fb_orig_base, p->fb_orig_size);
1409         kfree(p);
1410 }
1411
1412
1413 /*
1414  * Parse user speficied options (`video=controlfb:')
1415  */
1416 void __init control_setup(char *options)
1417 {
1418         char *this_opt;
1419
1420         if (!options || !*options)
1421                 return;
1422
1423         while ((this_opt = strsep(&options, ",")) != NULL) {
1424                 if (!strncmp(this_opt, "font:", 5)) {
1425                         char *p;
1426                         int i;
1427
1428                         p = this_opt +5;
1429                         for (i = 0; i < sizeof(fontname) - 1; i++)
1430                                 if (!*p || *p == ' ' || *p == ',')
1431                                         break;
1432                         memcpy(fontname, this_opt + 5, i);
1433                         fontname[i] = 0;
1434                 } else if (!strncmp(this_opt, "vmode:", 6)) {
1435                         int vmode = simple_strtoul(this_opt+6, NULL, 0);
1436                         if (vmode > 0 && vmode <= VMODE_MAX &&
1437                             control_mac_modes[vmode - 1].m[1] >= 0)
1438                                 default_vmode = vmode;
1439                 } else if (!strncmp(this_opt, "cmode:", 6)) {
1440                         int depth = simple_strtoul(this_opt+6, NULL, 0);
1441                         switch (depth) {
1442                          case CMODE_8:
1443                          case CMODE_16:
1444                          case CMODE_32:
1445                                 default_cmode = depth;
1446                                 break;
1447                          case 8:
1448                                 default_cmode = CMODE_8;
1449                                 break;
1450                          case 15:
1451                          case 16:
1452                                 default_cmode = CMODE_16;
1453                                 break;
1454                          case 24:
1455                          case 32:
1456                                 default_cmode = CMODE_32;
1457                                 break;
1458                         }
1459                 }
1460         }
1461 }
1462