drivers/video/sm501fb.c

   1 /* linux/drivers/video/sm501fb.c
   2  *
   3  * Copyright (c) 2006 Simtec Electronics
   4  *      Vincent Sanders <vince@simtec.co.uk>
   5  *      Ben Dooks <ben@simtec.co.uk>
   6  *
   7  * This program is free software; you can redistribute it and/or modify
   8  * it under the terms of the GNU General Public License version 2 as
   9  * published by the Free Software Foundation.
  10  *
  11  * Framebuffer driver for the Silicon Motion SM501
  12  */
  13
  14 #include <linux/module.h>
  15 #include <linux/kernel.h>
  16 #include <linux/errno.h>
  17 #include <linux/string.h>
  18 #include <linux/mm.h>
  19 #include <linux/tty.h>
  20 #include <linux/slab.h>
  21 #include <linux/delay.h>
  22 #include <linux/fb.h>
  23 #include <linux/init.h>
  24 #include <linux/vmalloc.h>
  25 #include <linux/dma-mapping.h>
  26 #include <linux/interrupt.h>
  27 #include <linux/workqueue.h>
  28 #include <linux/wait.h>
  29 #include <linux/platform_device.h>
  30 #include <linux/clk.h>
  31 #include <linux/console.h>
  32 #include <linux/io.h>
  33
  34 #include <asm/uaccess.h>
  35 #include <asm/div64.h>
  36
  37 #ifdef CONFIG_PM
  38 #include <linux/pm.h>
  39 #endif
  40
  41 #include <linux/sm501.h>
  42 #include <linux/sm501-regs.h>
  43
  44 #define NR_PALETTE      256
  45
  46 enum sm501_controller {
  47         HEAD_CRT        = 0,
  48         HEAD_PANEL      = 1,
  49 };
  50
  51 /* SM501 memory address.
  52  *
  53  * This structure is used to track memory usage within the SM501 framebuffer
  54  * allocation. The sm_addr field is stored as an offset as it is often used
  55  * against both the physical and mapped addresses.
  56  */
  57 struct sm501_mem {
  58         unsigned long    size;
  59         unsigned long    sm_addr;       /* offset from base of sm501 fb. */
  60         void __iomem    *k_addr;
  61 };
  62
  63 /* private data that is shared between all frambuffers* */
  64 struct sm501fb_info {
  65         struct device           *dev;
  66         struct fb_info          *fb[2];         /* fb info for both heads */
  67         struct resource         *fbmem_res;     /* framebuffer resource */
  68         struct resource         *regs_res;      /* registers resource */
  69         struct resource         *regs2d_res;    /* 2d registers resource */
  70         struct sm501_platdata_fb *pdata;        /* our platform data */
  71
  72         unsigned long            pm_crt_ctrl;   /* pm: crt ctrl save */
  73
  74         int                      irq;
  75         int                      swap_endian;   /* set to swap rgb=>bgr */
  76         void __iomem            *regs;          /* remapped registers */
  77         void __iomem            *regs2d;        /* 2d remapped registers */
  78         void __iomem            *fbmem;         /* remapped framebuffer */
  79         size_t                   fbmem_len;     /* length of remapped region */
  80 };
  81
  82 /* per-framebuffer private data */
  83 struct sm501fb_par {
  84         u32                      pseudo_palette[16];
  85
  86         enum sm501_controller    head;
  87         struct sm501_mem         cursor;
  88         struct sm501_mem         screen;
  89         struct fb_ops            ops;
  90
  91         void                    *store_fb;
  92         void                    *store_cursor;
  93         void __iomem            *cursor_regs;
  94         struct sm501fb_info     *info;
  95 };
  96
  97 /* Helper functions */
  98
  99 static inline int h_total(struct fb_var_screeninfo *var)
 100 {
 101         return var->xres + var->left_margin +
 102                 var->right_margin + var->hsync_len;
 103 }
 104
 105 static inline int v_total(struct fb_var_screeninfo *var)
 106 {
 107         return var->yres + var->upper_margin +
 108                 var->lower_margin + var->vsync_len;
 109 }
 110
 111 /* sm501fb_sync_regs()
 112  *
 113  * This call is mainly for PCI bus systems where we need to
 114  * ensure that any writes to the bus are completed before the
 115  * next phase, or after completing a function.
 116 */
 117
 118 static inline void sm501fb_sync_regs(struct sm501fb_info *info)
 119 {
 120         readl(info->regs);
 121 }
 122
 123 /* sm501_alloc_mem
 124  *
 125  * This is an attempt to lay out memory for the two framebuffers and
 126  * everything else
 127  *
 128  * |fbmem_res->start                                           fbmem_res->end|
 129  * |                                                                         |
 130  * |fb[0].fix.smem_start    |         |fb[1].fix.smem_start    |     2K      |
 131  * |-> fb[0].fix.smem_len <-| spare   |-> fb[1].fix.smem_len <-|-> cursors <-|
 132  *
 133  * The "spare" space is for the 2d engine data
 134  * the fixed is space for the cursors (2x1Kbyte)
 135  *
 136  * we need to allocate memory for the 2D acceleration engine
 137  * command list and the data for the engine to deal with.
 138  *
 139  * - all allocations must be 128bit aligned
 140  * - cursors are 64x64x2 bits (1Kbyte)
 141  *
 142  */
 143
 144 #define SM501_MEMF_CURSOR               (1)
 145 #define SM501_MEMF_PANEL                (2)
 146 #define SM501_MEMF_CRT                  (4)
 147 #define SM501_MEMF_ACCEL                (8)
 148
 149 static int sm501_alloc_mem(struct sm501fb_info *inf, struct sm501_mem *mem,
 150                            unsigned int why, size_t size, u32 smem_len)
 151 {
 152         struct sm501fb_par *par;
 153         struct fb_info *fbi;
 154         unsigned int ptr;
 155         unsigned int end;
 156
 157         switch (why) {
 158         case SM501_MEMF_CURSOR:
 159                 ptr = inf->fbmem_len - size;
 160                 inf->fbmem_len = ptr;   /* adjust available memory. */
 161                 break;
 162
 163         case SM501_MEMF_PANEL:
 164                 if (size > inf->fbmem_len)
 165                         return -ENOMEM;
 166
 167                 ptr = inf->fbmem_len - size;
 168                 fbi = inf->fb[HEAD_CRT];
 169
 170                 /* round down, some programs such as directfb do not draw
 171                  * 0,0 correctly unless the start is aligned to a page start.
 172                  */
 173
 174                 if (ptr > 0)
 175                         ptr &= ~(PAGE_SIZE - 1);
 176
 177                 if (fbi && ptr < smem_len)
 178                         return -ENOMEM;
 179
 180                 break;
 181
 182         case SM501_MEMF_CRT:
 183                 ptr = 0;
 184
 185                 /* check to see if we have panel memory allocated
 186                  * which would put an limit on available memory. */
 187
 188                 fbi = inf->fb[HEAD_PANEL];
 189                 if (fbi) {
 190                         par = fbi->par;
 191                         end = par->screen.k_addr ? par->screen.sm_addr : inf->fbmem_len;
 192                 } else
 193                         end = inf->fbmem_len;
 194
 195                 if ((ptr + size) > end)
 196                         return -ENOMEM;
 197
 198                 break;
 199
 200         case SM501_MEMF_ACCEL:
 201                 fbi = inf->fb[HEAD_CRT];
 202                 ptr = fbi ? smem_len : 0;
 203
 204                 fbi = inf->fb[HEAD_PANEL];
 205                 if (fbi) {
 206                         par = fbi->par;
 207                         end = par->screen.sm_addr;
 208                 } else
 209                         end = inf->fbmem_len;
 210
 211                 if ((ptr + size) > end)
 212                         return -ENOMEM;
 213
 214                 break;
 215
 216         default:
 217                 return -EINVAL;
 218         }
 219
 220         mem->size    = size;
 221         mem->sm_addr = ptr;
 222         mem->k_addr  = inf->fbmem + ptr;
 223
 224         dev_dbg(inf->dev, "%s: result %08lx, %p - %u, %zd\n",
 225                 __func__, mem->sm_addr, mem->k_addr, why, size);
 226
 227         return 0;
 228 }
 229
 230 /* sm501fb_ps_to_hz
 231  *
 232  * Converts a period in picoseconds to Hz.
 233  *
 234  * Note, we try to keep this in Hz to minimise rounding with
 235  * the limited PLL settings on the SM501.
 236 */
 237
 238 static unsigned long sm501fb_ps_to_hz(unsigned long psvalue)
 239 {
 240         unsigned long long numerator=1000000000000ULL;
 241
 242         /* 10^12 / picosecond period gives frequency in Hz */
 243         do_div(numerator, psvalue);
 244         return (unsigned long)numerator;
 245 }
 246
 247 /* sm501fb_hz_to_ps is identical to the oposite transform */
 248
 249 #define sm501fb_hz_to_ps(x) sm501fb_ps_to_hz(x)
 250
 251 /* sm501fb_setup_gamma
 252  *
 253  * Programs a linear 1.0 gamma ramp in case the gamma
 254  * correction is enabled without programming anything else.
 255 */
 256
 257 static void sm501fb_setup_gamma(struct sm501fb_info *fbi,
 258                                 unsigned long palette)
 259 {
 260         unsigned long value = 0;
 261         int offset;
 262
 263         /* set gamma values */
 264         for (offset = 0; offset < 256 * 4; offset += 4) {
 265                 writel(value, fbi->regs + palette + offset);
 266                 value += 0x010101;      /* Advance RGB by 1,1,1.*/
 267         }
 268 }
 269
 270 /* sm501fb_check_var
 271  *
 272  * check common variables for both panel and crt
 273 */
 274
 275 static int sm501fb_check_var(struct fb_var_screeninfo *var,
 276                              struct fb_info *info)
 277 {
 278         struct sm501fb_par  *par = info->par;
 279         struct sm501fb_info *sm  = par->info;
 280         unsigned long tmp;
 281
 282         /* check we can fit these values into the registers */
 283
 284         if (var->hsync_len > 255 || var->vsync_len > 63)
 285                 return -EINVAL;
 286
 287         /* hdisplay end and hsync start */
 288         if ((var->xres + var->right_margin) > 4096)
 289                 return -EINVAL;
 290
 291         /* vdisplay end and vsync start */
 292         if ((var->yres + var->lower_margin) > 2048)
 293                 return -EINVAL;
 294
 295         /* hard limits of device */
 296
 297         if (h_total(var) > 4096 || v_total(var) > 2048)
 298                 return -EINVAL;
 299
 300         /* check our line length is going to be 128 bit aligned */
 301
 302         tmp = (var->xres * var->bits_per_pixel) / 8;
 303         if ((tmp & 15) != 0)
 304                 return -EINVAL;
 305
 306         /* check the virtual size */
 307
 308         if (var->xres_virtual > 4096 || var->yres_virtual > 2048)
 309                 return -EINVAL;
 310
 311         /* can cope with 8,16 or 32bpp */
 312
 313         if (var->bits_per_pixel <= 8)
 314                 var->bits_per_pixel = 8;
 315         else if (var->bits_per_pixel <= 16)
 316                 var->bits_per_pixel = 16;
 317         else if (var->bits_per_pixel == 24)
 318                 var->bits_per_pixel = 32;
 319
 320         /* set r/g/b positions and validate bpp */
 321         switch(var->bits_per_pixel) {
 322         case 8:
 323                 var->red.length         = var->bits_per_pixel;
 324                 var->red.offset         = 0;
 325                 var->green.length       = var->bits_per_pixel;
 326                 var->green.offset       = 0;
 327                 var->blue.length        = var->bits_per_pixel;
 328                 var->blue.offset        = 0;
 329                 var->transp.length      = 0;
 330                 var->transp.offset      = 0;
 331
 332                 break;
 333
 334         case 16:
 335                 if (sm->pdata->flags & SM501_FBPD_SWAP_FB_ENDIAN) {
 336                         var->blue.offset        = 11;
 337                         var->green.offset       = 5;
 338                         var->red.offset         = 0;
 339                 } else {
 340                         var->red.offset         = 11;
 341                         var->green.offset       = 5;
 342                         var->blue.offset        = 0;
 343                 }
 344                 var->transp.offset      = 0;
 345
 346                 var->red.length         = 5;
 347                 var->green.length       = 6;
 348                 var->blue.length        = 5;
 349                 var->transp.length      = 0;
 350                 break;
 351
 352         case 32:
 353                 if (sm->pdata->flags & SM501_FBPD_SWAP_FB_ENDIAN) {
 354                         var->transp.offset      = 0;
 355                         var->red.offset         = 8;
 356                         var->green.offset       = 16;
 357                         var->blue.offset        = 24;
 358                 } else {
 359                         var->transp.offset      = 24;
 360                         var->red.offset         = 16;
 361                         var->green.offset       = 8;
 362                         var->blue.offset        = 0;
 363                 }
 364
 365                 var->red.length         = 8;
 366                 var->green.length       = 8;
 367                 var->blue.length        = 8;
 368                 var->transp.length      = 0;
 369                 break;
 370
 371         default:
 372                 return -EINVAL;
 373         }
 374
 375         return 0;
 376 }
 377
 378 /*
 379  * sm501fb_check_var_crt():
 380  *
 381  * check the parameters for the CRT head, and either bring them
 382  * back into range, or return -EINVAL.
 383 */
 384
 385 static int sm501fb_check_var_crt(struct fb_var_screeninfo *var,
 386                                  struct fb_info *info)
 387 {
 388         return sm501fb_check_var(var, info);
 389 }
 390
 391 /* sm501fb_check_var_pnl():
 392  *
 393  * check the parameters for the CRT head, and either bring them
 394  * back into range, or return -EINVAL.
 395 */
 396
 397 static int sm501fb_check_var_pnl(struct fb_var_screeninfo *var,
 398                                  struct fb_info *info)
 399 {
 400         return sm501fb_check_var(var, info);
 401 }
 402
 403 /* sm501fb_set_par_common
 404  *
 405  * set common registers for framebuffers
 406 */
 407
 408 static int sm501fb_set_par_common(struct fb_info *info,
 409                                   struct fb_var_screeninfo *var)
 410 {
 411         struct sm501fb_par  *par = info->par;
 412         struct sm501fb_info *fbi = par->info;
 413         unsigned long pixclock;      /* pixelclock in Hz */
 414         unsigned long sm501pixclock; /* pixelclock the 501 can achieve in Hz */
 415         unsigned int mem_type;
 416         unsigned int clock_type;
 417         unsigned int head_addr;
 418         unsigned int smem_len;
 419
 420         dev_dbg(fbi->dev, "%s: %dx%d, bpp = %d, virtual %dx%d\n",
 421                 __func__, var->xres, var->yres, var->bits_per_pixel,
 422                 var->xres_virtual, var->yres_virtual);
 423
 424         switch (par->head) {
 425         case HEAD_CRT:
 426                 mem_type = SM501_MEMF_CRT;
 427                 clock_type = SM501_CLOCK_V2XCLK;
 428                 head_addr = SM501_DC_CRT_FB_ADDR;
 429                 break;
 430
 431         case HEAD_PANEL:
 432                 mem_type = SM501_MEMF_PANEL;
 433                 clock_type = SM501_CLOCK_P2XCLK;
 434                 head_addr = SM501_DC_PANEL_FB_ADDR;
 435                 break;
 436
 437         default:
 438                 mem_type = 0;           /* stop compiler warnings */
 439                 head_addr = 0;
 440                 clock_type = 0;
 441         }
 442
 443         switch (var->bits_per_pixel) {
 444         case 8:
 445                 info->fix.visual = FB_VISUAL_PSEUDOCOLOR;
 446                 break;
 447
 448         case 16:
 449                 info->fix.visual = FB_VISUAL_TRUECOLOR;
 450                 break;
 451
 452         case 32:
 453                 info->fix.visual = FB_VISUAL_TRUECOLOR;
 454                 break;
 455         }
 456
 457         /* allocate fb memory within 501 */
 458         info->fix.line_length = (var->xres_virtual * var->bits_per_pixel)/8;
 459         smem_len = info->fix.line_length * var->yres_virtual;
 460
 461         dev_dbg(fbi->dev, "%s: line length = %u\n", __func__,
 462                 info->fix.line_length);
 463
 464         if (sm501_alloc_mem(fbi, &par->screen, mem_type, smem_len, smem_len)) {
 465                 dev_err(fbi->dev, "no memory available\n");
 466                 return -ENOMEM;
 467         }
 468
 469         mutex_lock(&info->mm_lock);
 470         info->fix.smem_start = fbi->fbmem_res->start + par->screen.sm_addr;
 471         info->fix.smem_len   = smem_len;
 472         mutex_unlock(&info->mm_lock);
 473
 474         info->screen_base = fbi->fbmem + par->screen.sm_addr;
 475         info->screen_size = info->fix.smem_len;
 476
 477         /* set start of framebuffer to the screen */
 478
 479         writel(par->screen.sm_addr | SM501_ADDR_FLIP, fbi->regs + head_addr);
 480
 481         /* program CRT clock  */
 482
 483         pixclock = sm501fb_ps_to_hz(var->pixclock);
 484
 485         sm501pixclock = sm501_set_clock(fbi->dev->parent, clock_type,
 486                                         pixclock);
 487
 488         /* update fb layer with actual clock used */
 489         var->pixclock = sm501fb_hz_to_ps(sm501pixclock);
 490
 491         dev_dbg(fbi->dev, "%s: pixclock(ps) = %u, pixclock(Hz)  = %lu, "
 492                "sm501pixclock = %lu,  error = %ld%%\n",
 493                __func__, var->pixclock, pixclock, sm501pixclock,
 494                ((pixclock - sm501pixclock)*100)/pixclock);
 495
 496         return 0;
 497 }
 498
 499 /* sm501fb_set_par_geometry
 500  *
 501  * set the geometry registers for specified framebuffer.
 502 */
 503
 504 static void sm501fb_set_par_geometry(struct fb_info *info,
 505                                      struct fb_var_screeninfo *var)
 506 {
 507         struct sm501fb_par  *par = info->par;
 508         struct sm501fb_info *fbi = par->info;
 509         void __iomem *base = fbi->regs;
 510         unsigned long reg;
 511
 512         if (par->head == HEAD_CRT)
 513                 base += SM501_DC_CRT_H_TOT;
 514         else
 515                 base += SM501_DC_PANEL_H_TOT;
 516
 517         /* set framebuffer width and display width */
 518
 519         reg = info->fix.line_length;
 520         reg |= ((var->xres * var->bits_per_pixel)/8) << 16;
 521
 522         writel(reg, fbi->regs + (par->head == HEAD_CRT ?
 523                     SM501_DC_CRT_FB_OFFSET :  SM501_DC_PANEL_FB_OFFSET));
 524
 525         /* program horizontal total */
 526
 527         reg  = (h_total(var) - 1) << 16;
 528         reg |= (var->xres - 1);
 529
 530         writel(reg, base + SM501_OFF_DC_H_TOT);
 531
 532         /* program horizontal sync */
 533
 534         reg  = var->hsync_len << 16;
 535         reg |= var->xres + var->right_margin - 1;
 536
 537         writel(reg, base + SM501_OFF_DC_H_SYNC);
 538
 539         /* program vertical total */
 540
 541         reg  = (v_total(var) - 1) << 16;
 542         reg |= (var->yres - 1);
 543
 544         writel(reg, base + SM501_OFF_DC_V_TOT);
 545
 546         /* program vertical sync */
 547         reg  = var->vsync_len << 16;
 548         reg |= var->yres + var->lower_margin - 1;
 549
 550         writel(reg, base + SM501_OFF_DC_V_SYNC);
 551 }
 552
 553 /* sm501fb_pan_crt
 554  *
 555  * pan the CRT display output within an virtual framebuffer
 556 */
 557
 558 static int sm501fb_pan_crt(struct fb_var_screeninfo *var,
 559                            struct fb_info *info)
 560 {
 561         struct sm501fb_par  *par = info->par;
 562         struct sm501fb_info *fbi = par->info;
 563         unsigned int bytes_pixel = var->bits_per_pixel / 8;
 564         unsigned long reg;
 565         unsigned long xoffs;
 566
 567         xoffs = var->xoffset * bytes_pixel;
 568
 569         reg = readl(fbi->regs + SM501_DC_CRT_CONTROL);
 570
 571         reg &= ~SM501_DC_CRT_CONTROL_PIXEL_MASK;
 572         reg |= ((xoffs & 15) / bytes_pixel) << 4;
 573         writel(reg, fbi->regs + SM501_DC_CRT_CONTROL);
 574
 575         reg = (par->screen.sm_addr + xoffs +
 576                var->yoffset * info->fix.line_length);
 577         writel(reg | SM501_ADDR_FLIP, fbi->regs + SM501_DC_CRT_FB_ADDR);
 578
 579         sm501fb_sync_regs(fbi);
 580         return 0;
 581 }
 582
 583 /* sm501fb_pan_pnl
 584  *
 585  * pan the panel display output within an virtual framebuffer
 586 */
 587
 588 static int sm501fb_pan_pnl(struct fb_var_screeninfo *var,
 589                            struct fb_info *info)
 590 {
 591         struct sm501fb_par  *par = info->par;
 592         struct sm501fb_info *fbi = par->info;
 593         unsigned long reg;
 594
 595         reg = var->xoffset | (var->xres_virtual << 16);
 596         writel(reg, fbi->regs + SM501_DC_PANEL_FB_WIDTH);
 597
 598         reg = var->yoffset | (var->yres_virtual << 16);
 599         writel(reg, fbi->regs + SM501_DC_PANEL_FB_HEIGHT);
 600
 601         sm501fb_sync_regs(fbi);
 602         return 0;
 603 }
 604
 605 /* sm501fb_set_par_crt
 606  *
 607  * Set the CRT video mode from the fb_info structure
 608 */
 609
 610 static int sm501fb_set_par_crt(struct fb_info *info)
 611 {
 612         struct sm501fb_par  *par = info->par;
 613         struct sm501fb_info *fbi = par->info;
 614         struct fb_var_screeninfo *var = &info->var;
 615         unsigned long control;       /* control register */
 616         int ret;
 617
 618         /* activate new configuration */
 619
 620         dev_dbg(fbi->dev, "%s(%p)\n", __func__, info);
 621
 622         /* enable CRT DAC - note 0 is on!*/
 623         sm501_misc_control(fbi->dev->parent, 0, SM501_MISC_DAC_POWER);
 624
 625         control = readl(fbi->regs + SM501_DC_CRT_CONTROL);
 626
 627         control &= (SM501_DC_CRT_CONTROL_PIXEL_MASK |
 628                     SM501_DC_CRT_CONTROL_GAMMA |
 629                     SM501_DC_CRT_CONTROL_BLANK |
 630                     SM501_DC_CRT_CONTROL_SEL |
 631                     SM501_DC_CRT_CONTROL_CP |
 632                     SM501_DC_CRT_CONTROL_TVP);
 633
 634         /* set the sync polarities before we check data source  */
 635
 636         if ((var->sync & FB_SYNC_HOR_HIGH_ACT) == 0)
 637                 control |= SM501_DC_CRT_CONTROL_HSP;
 638
 639         if ((var->sync & FB_SYNC_VERT_HIGH_ACT) == 0)
 640                 control |= SM501_DC_CRT_CONTROL_VSP;
 641
 642         if ((control & SM501_DC_CRT_CONTROL_SEL) == 0) {
 643                 /* the head is displaying panel data... */
 644
 645                 sm501_alloc_mem(fbi, &par->screen, SM501_MEMF_CRT, 0,
 646                                 info->fix.smem_len);
 647                 goto out_update;
 648         }
 649
 650         ret = sm501fb_set_par_common(info, var);
 651         if (ret) {
 652                 dev_err(fbi->dev, "failed to set common parameters\n");
 653                 return ret;
 654         }
 655
 656         sm501fb_pan_crt(var, info);
 657         sm501fb_set_par_geometry(info, var);
 658
 659         control |= SM501_FIFO_3;        /* fill if >3 free slots */
 660
 661         switch(var->bits_per_pixel) {
 662         case 8:
 663                 control |= SM501_DC_CRT_CONTROL_8BPP;
 664                 break;
 665
 666         case 16:
 667                 control |= SM501_DC_CRT_CONTROL_16BPP;
 668                 sm501fb_setup_gamma(fbi, SM501_DC_CRT_PALETTE);
 669                 break;
 670
 671         case 32:
 672                 control |= SM501_DC_CRT_CONTROL_32BPP;
 673                 sm501fb_setup_gamma(fbi, SM501_DC_CRT_PALETTE);
 674                 break;
 675
 676         default:
 677                 BUG();
 678         }
 679
 680         control |= SM501_DC_CRT_CONTROL_SEL;    /* CRT displays CRT data */
 681         control |= SM501_DC_CRT_CONTROL_TE;     /* enable CRT timing */
 682         control |= SM501_DC_CRT_CONTROL_ENABLE; /* enable CRT plane */
 683
 684  out_update:
 685         dev_dbg(fbi->dev, "new control is %08lx\n", control);
 686
 687         writel(control, fbi->regs + SM501_DC_CRT_CONTROL);
 688         sm501fb_sync_regs(fbi);
 689
 690         return 0;
 691 }
 692
 693 static void sm501fb_panel_power(struct sm501fb_info *fbi, int to)
 694 {
 695         unsigned long control;
 696         void __iomem *ctrl_reg = fbi->regs + SM501_DC_PANEL_CONTROL;
 697         struct sm501_platdata_fbsub *pd = fbi->pdata->fb_pnl;
 698
 699         control = readl(ctrl_reg);
 700
 701         if (to && (control & SM501_DC_PANEL_CONTROL_VDD) == 0) {
 702                 /* enable panel power */
 703
 704                 control |= SM501_DC_PANEL_CONTROL_VDD;  /* FPVDDEN */
 705                 writel(control, ctrl_reg);
 706                 sm501fb_sync_regs(fbi);
 707                 mdelay(10);
 708
 709                 control |= SM501_DC_PANEL_CONTROL_DATA; /* DATA */
 710                 writel(control, ctrl_reg);
 711                 sm501fb_sync_regs(fbi);
 712                 mdelay(10);
 713
 714                 /* VBIASEN */
 715
 716                 if (!(pd->flags & SM501FB_FLAG_PANEL_NO_VBIASEN)) {
 717                         if (pd->flags & SM501FB_FLAG_PANEL_INV_VBIASEN)
 718                                 control &= ~SM501_DC_PANEL_CONTROL_BIAS;
 719                         else
 720                                 control |= SM501_DC_PANEL_CONTROL_BIAS;
 721
 722                         writel(control, ctrl_reg);
 723                         sm501fb_sync_regs(fbi);
 724                         mdelay(10);
 725                 }
 726
 727                 if (!(pd->flags & SM501FB_FLAG_PANEL_NO_FPEN)) {
 728                         if (pd->flags & SM501FB_FLAG_PANEL_INV_FPEN)
 729                                 control &= ~SM501_DC_PANEL_CONTROL_FPEN;
 730                         else
 731                                 control |= SM501_DC_PANEL_CONTROL_FPEN;
 732
 733                         writel(control, ctrl_reg);
 734                         sm501fb_sync_regs(fbi);
 735                         mdelay(10);
 736                 }
 737         } else if (!to && (control & SM501_DC_PANEL_CONTROL_VDD) != 0) {
 738                 /* disable panel power */
 739                 if (!(pd->flags & SM501FB_FLAG_PANEL_NO_FPEN)) {
 740                         if (pd->flags & SM501FB_FLAG_PANEL_INV_FPEN)
 741                                 control |= SM501_DC_PANEL_CONTROL_FPEN;
 742                         else
 743                                 control &= ~SM501_DC_PANEL_CONTROL_FPEN;
 744
 745                         writel(control, ctrl_reg);
 746                         sm501fb_sync_regs(fbi);
 747                         mdelay(10);
 748                 }
 749
 750                 if (!(pd->flags & SM501FB_FLAG_PANEL_NO_VBIASEN)) {
 751                         if (pd->flags & SM501FB_FLAG_PANEL_INV_VBIASEN)
 752                                 control |= SM501_DC_PANEL_CONTROL_BIAS;
 753                         else
 754                                 control &= ~SM501_DC_PANEL_CONTROL_BIAS;
 755
 756                         writel(control, ctrl_reg);
 757                         sm501fb_sync_regs(fbi);
 758                         mdelay(10);
 759                 }
 760
 761                 control &= ~SM501_DC_PANEL_CONTROL_DATA;
 762                 writel(control, ctrl_reg);
 763                 sm501fb_sync_regs(fbi);
 764                 mdelay(10);
 765
 766                 control &= ~SM501_DC_PANEL_CONTROL_VDD;
 767                 writel(control, ctrl_reg);
 768                 sm501fb_sync_regs(fbi);
 769                 mdelay(10);
 770         }
 771
 772         sm501fb_sync_regs(fbi);
 773 }
 774
 775 /* sm501fb_set_par_pnl
 776  *
 777  * Set the panel video mode from the fb_info structure
 778 */
 779
 780 static int sm501fb_set_par_pnl(struct fb_info *info)
 781 {
 782         struct sm501fb_par  *par = info->par;
 783         struct sm501fb_info *fbi = par->info;
 784         struct fb_var_screeninfo *var = &info->var;
 785         unsigned long control;
 786         unsigned long reg;
 787         int ret;
 788
 789         dev_dbg(fbi->dev, "%s(%p)\n", __func__, info);
 790
 791         /* activate this new configuration */
 792
 793         ret = sm501fb_set_par_common(info, var);
 794         if (ret)
 795                 return ret;
 796
 797         sm501fb_pan_pnl(var, info);
 798         sm501fb_set_par_geometry(info, var);
 799
 800         /* update control register */
 801
 802         control = readl(fbi->regs + SM501_DC_PANEL_CONTROL);
 803         control &= (SM501_DC_PANEL_CONTROL_GAMMA |
 804                     SM501_DC_PANEL_CONTROL_VDD  |
 805                     SM501_DC_PANEL_CONTROL_DATA |
 806                     SM501_DC_PANEL_CONTROL_BIAS |
 807                     SM501_DC_PANEL_CONTROL_FPEN |
 808                     SM501_DC_PANEL_CONTROL_CP |
 809                     SM501_DC_PANEL_CONTROL_CK |
 810                     SM501_DC_PANEL_CONTROL_HP |
 811                     SM501_DC_PANEL_CONTROL_VP |
 812                     SM501_DC_PANEL_CONTROL_HPD |
 813                     SM501_DC_PANEL_CONTROL_VPD);
 814
 815         control |= SM501_FIFO_3;        /* fill if >3 free slots */
 816
 817         switch(var->bits_per_pixel) {
 818         case 8:
 819                 control |= SM501_DC_PANEL_CONTROL_8BPP;
 820                 break;
 821
 822         case 16:
 823                 control |= SM501_DC_PANEL_CONTROL_16BPP;
 824                 sm501fb_setup_gamma(fbi, SM501_DC_PANEL_PALETTE);
 825                 break;
 826
 827         case 32:
 828                 control |= SM501_DC_PANEL_CONTROL_32BPP;
 829                 sm501fb_setup_gamma(fbi, SM501_DC_PANEL_PALETTE);
 830                 break;
 831
 832         default:
 833                 BUG();
 834         }
 835
 836         writel(0x0, fbi->regs + SM501_DC_PANEL_PANNING_CONTROL);
 837
 838         /* panel plane top left and bottom right location */
 839
 840         writel(0x00, fbi->regs + SM501_DC_PANEL_TL_LOC);
 841
 842         reg  = var->xres - 1;
 843         reg |= (var->yres - 1) << 16;
 844
 845         writel(reg, fbi->regs + SM501_DC_PANEL_BR_LOC);
 846
 847         /* program panel control register */
 848
 849         control |= SM501_DC_PANEL_CONTROL_TE;   /* enable PANEL timing */
 850         control |= SM501_DC_PANEL_CONTROL_EN;   /* enable PANEL gfx plane */
 851
 852         if ((var->sync & FB_SYNC_HOR_HIGH_ACT) == 0)
 853                 control |= SM501_DC_PANEL_CONTROL_HSP;
 854
 855         if ((var->sync & FB_SYNC_VERT_HIGH_ACT) == 0)
 856                 control |= SM501_DC_PANEL_CONTROL_VSP;
 857
 858         writel(control, fbi->regs + SM501_DC_PANEL_CONTROL);
 859         sm501fb_sync_regs(fbi);
 860
 861         /* ensure the panel interface is not tristated at this point */
 862
 863         sm501_modify_reg(fbi->dev->parent, SM501_SYSTEM_CONTROL,
 864                          0, SM501_SYSCTRL_PANEL_TRISTATE);
 865
 866         /* power the panel up */
 867         sm501fb_panel_power(fbi, 1);
 868         return 0;
 869 }
 870
 871
 872 /* chan_to_field
 873  *
 874  * convert a colour value into a field position
 875  *
 876  * from pxafb.c
 877 */
 878
 879 static inline unsigned int chan_to_field(unsigned int chan,
 880                                          struct fb_bitfield *bf)
 881 {
 882         chan &= 0xffff;
 883         chan >>= 16 - bf->length;
 884         return chan << bf->offset;
 885 }
 886
 887 /* sm501fb_setcolreg
 888  *
 889  * set the colour mapping for modes that support palettised data
 890 */
 891
 892 static int sm501fb_setcolreg(unsigned regno,
 893                              unsigned red, unsigned green, unsigned blue,
 894                              unsigned transp, struct fb_info *info)
 895 {
 896         struct sm501fb_par  *par = info->par;
 897         struct sm501fb_info *fbi = par->info;
 898         void __iomem *base = fbi->regs;
 899         unsigned int val;
 900
 901         if (par->head == HEAD_CRT)
 902                 base += SM501_DC_CRT_PALETTE;
 903         else
 904                 base += SM501_DC_PANEL_PALETTE;
 905
 906         switch (info->fix.visual) {
 907         case FB_VISUAL_TRUECOLOR:
 908                 /* true-colour, use pseuo-palette */
 909
 910                 if (regno < 16) {
 911                         u32 *pal = par->pseudo_palette;
 912
 913                         val  = chan_to_field(red,   &info->var.red);
 914                         val |= chan_to_field(green, &info->var.green);
 915                         val |= chan_to_field(blue,  &info->var.blue);
 916
 917                         pal[regno] = val;
 918                 }
 919                 break;
 920
 921         case FB_VISUAL_PSEUDOCOLOR:
 922                 if (regno < 256) {
 923                         val = (red >> 8) << 16;
 924                         val |= (green >> 8) << 8;
 925                         val |= blue >> 8;
 926
 927                         writel(val, base + (regno * 4));
 928                 }
 929
 930                 break;
 931
 932         default:
 933                 return 1;   /* unknown type */
 934         }
 935
 936         return 0;
 937 }
 938
 939 /* sm501fb_blank_pnl
 940  *
 941  * Blank or un-blank the panel interface
 942 */
 943
 944 static int sm501fb_blank_pnl(int blank_mode, struct fb_info *info)
 945 {
 946         struct sm501fb_par  *par = info->par;
 947         struct sm501fb_info *fbi = par->info;
 948
 949         dev_dbg(fbi->dev, "%s(mode=%d, %p)\n", __func__, blank_mode, info);
 950
 951         switch (blank_mode) {
 952         case FB_BLANK_POWERDOWN:
 953                 sm501fb_panel_power(fbi, 0);
 954                 break;
 955
 956         case FB_BLANK_UNBLANK:
 957                 sm501fb_panel_power(fbi, 1);
 958                 break;
 959
 960         case FB_BLANK_NORMAL:
 961         case FB_BLANK_VSYNC_SUSPEND:
 962         case FB_BLANK_HSYNC_SUSPEND:
 963         default:
 964                 return 1;
 965         }
 966
 967         return 0;
 968 }
 969
 970 /* sm501fb_blank_crt
 971  *
 972  * Blank or un-blank the crt interface
 973 */
 974
 975 static int sm501fb_blank_crt(int blank_mode, struct fb_info *info)
 976 {
 977         struct sm501fb_par  *par = info->par;
 978         struct sm501fb_info *fbi = par->info;
 979         unsigned long ctrl;
 980
 981         dev_dbg(fbi->dev, "%s(mode=%d, %p)\n", __func__, blank_mode, info);
 982
 983         ctrl = readl(fbi->regs + SM501_DC_CRT_CONTROL);
 984
 985         switch (blank_mode) {
 986         case FB_BLANK_POWERDOWN:
 987                 ctrl &= ~SM501_DC_CRT_CONTROL_ENABLE;
 988                 sm501_misc_control(fbi->dev->parent, SM501_MISC_DAC_POWER, 0);
 989
 990         case FB_BLANK_NORMAL:
 991                 ctrl |= SM501_DC_CRT_CONTROL_BLANK;
 992                 break;
 993
 994         case FB_BLANK_UNBLANK:
 995                 ctrl &= ~SM501_DC_CRT_CONTROL_BLANK;
 996                 ctrl |=  SM501_DC_CRT_CONTROL_ENABLE;
 997                 sm501_misc_control(fbi->dev->parent, 0, SM501_MISC_DAC_POWER);
 998                 break;
 999
1000         case FB_BLANK_VSYNC_SUSPEND:
1001         case FB_BLANK_HSYNC_SUSPEND:
1002         default:
1003                 return 1;
1004
1005         }
1006
1007         writel(ctrl, fbi->regs + SM501_DC_CRT_CONTROL);
1008         sm501fb_sync_regs(fbi);
1009
1010         return 0;
1011 }
1012
1013 /* sm501fb_cursor
1014  *
1015  * set or change the hardware cursor parameters
1016 */
1017
1018 static int sm501fb_cursor(struct fb_info *info, struct fb_cursor *cursor)
1019 {
1020         struct sm501fb_par  *par = info->par;
1021         struct sm501fb_info *fbi = par->info;
1022         void __iomem *base = fbi->regs;
1023         unsigned long hwc_addr;
1024         unsigned long fg, bg;
1025
1026         dev_dbg(fbi->dev, "%s(%p,%p)\n", __func__, info, cursor);
1027
1028         if (par->head == HEAD_CRT)
1029                 base += SM501_DC_CRT_HWC_BASE;
1030         else
1031                 base += SM501_DC_PANEL_HWC_BASE;
1032
1033         /* check not being asked to exceed capabilities */
1034
1035         if (cursor->image.width > 64)
1036                 return -EINVAL;
1037
1038         if (cursor->image.height > 64)
1039                 return -EINVAL;
1040
1041         if (cursor->image.depth > 1)
1042                 return -EINVAL;
1043
1044         hwc_addr = readl(base + SM501_OFF_HWC_ADDR);
1045
1046         if (cursor->enable)
1047                 writel(hwc_addr | SM501_HWC_EN, base + SM501_OFF_HWC_ADDR);
1048         else
1049                 writel(hwc_addr & ~SM501_HWC_EN, base + SM501_OFF_HWC_ADDR);
1050
1051         /* set data */
1052         if (cursor->set & FB_CUR_SETPOS) {
1053                 unsigned int x = cursor->image.dx;
1054                 unsigned int y = cursor->image.dy;
1055
1056                 if (x >= 2048 || y >= 2048 )
1057                         return -EINVAL;
1058
1059                 dev_dbg(fbi->dev, "set position %d,%d\n", x, y);
1060
1061                 //y += cursor->image.height;
1062
1063                 writel(x | (y << 16), base + SM501_OFF_HWC_LOC);
1064         }
1065
1066         if (cursor->set & FB_CUR_SETCMAP) {
1067                 unsigned int bg_col = cursor->image.bg_color;
1068                 unsigned int fg_col = cursor->image.fg_color;
1069
1070                 dev_dbg(fbi->dev, "%s: update cmap (%08x,%08x)\n",
1071                         __func__, bg_col, fg_col);
1072
1073                 bg = ((info->cmap.red[bg_col] & 0xF8) << 8) |
1074                         ((info->cmap.green[bg_col] & 0xFC) << 3) |
1075                         ((info->cmap.blue[bg_col] & 0xF8) >> 3);
1076
1077                 fg = ((info->cmap.red[fg_col] & 0xF8) << 8) |
1078                         ((info->cmap.green[fg_col] & 0xFC) << 3) |
1079                         ((info->cmap.blue[fg_col] & 0xF8) >> 3);
1080
1081                 dev_dbg(fbi->dev, "fgcol %08lx, bgcol %08lx\n", fg, bg);
1082
1083                 writel(bg, base + SM501_OFF_HWC_COLOR_1_2);
1084                 writel(fg, base + SM501_OFF_HWC_COLOR_3);
1085         }
1086
1087         if (cursor->set & FB_CUR_SETSIZE ||
1088             cursor->set & (FB_CUR_SETIMAGE | FB_CUR_SETSHAPE)) {
1089                 /* SM501 cursor is a two bpp 64x64 bitmap this routine
1090                  * clears it to transparent then combines the cursor
1091                  * shape plane with the colour plane to set the
1092                  * cursor */
1093                 int x, y;
1094                 const unsigned char *pcol = cursor->image.data;
1095                 const unsigned char *pmsk = cursor->mask;
1096                 void __iomem   *dst = par->cursor.k_addr;
1097                 unsigned char  dcol = 0;
1098                 unsigned char  dmsk = 0;
1099                 unsigned int   op;
1100
1101                 dev_dbg(fbi->dev, "%s: setting shape (%d,%d)\n",
1102                         __func__, cursor->image.width, cursor->image.height);
1103
1104                 for (op = 0; op < (64*64*2)/8; op+=4)
1105                         writel(0x0, dst + op);
1106
1107                 for (y = 0; y < cursor->image.height; y++) {
1108                         for (x = 0; x < cursor->image.width; x++) {
1109                                 if ((x % 8) == 0) {
1110                                         dcol = *pcol++;
1111                                         dmsk = *pmsk++;
1112                                 } else {
1113                                         dcol >>= 1;
1114                                         dmsk >>= 1;
1115                                 }
1116
1117                                 if (dmsk & 1) {
1118                                         op = (dcol & 1) ? 1 : 3;
1119                                         op <<= ((x % 4) * 2);
1120
1121                                         op |= readb(dst + (x / 4));
1122                                         writeb(op, dst + (x / 4));
1123                                 }
1124                         }
1125                         dst += (64*2)/8;
1126                 }
1127         }
1128
1129         sm501fb_sync_regs(fbi); /* ensure cursor data flushed */
1130         return 0;
1131 }
1132
1133 /* sm501fb_crtsrc_show
1134  *
1135  * device attribute code to show where the crt output is sourced from
1136 */
1137
1138 static ssize_t sm501fb_crtsrc_show(struct device *dev,
1139                                struct device_attribute *attr, char *buf)
1140 {
1141         struct sm501fb_info *info = dev_get_drvdata(dev);
1142         unsigned long ctrl;
1143
1144         ctrl = readl(info->regs + SM501_DC_CRT_CONTROL);
1145         ctrl &= SM501_DC_CRT_CONTROL_SEL;
1146
1147         return snprintf(buf, PAGE_SIZE, "%s\n", ctrl ? "crt" : "panel");
1148 }
1149
1150 /* sm501fb_crtsrc_show
1151  *
1152  * device attribute code to set where the crt output is sourced from
1153 */
1154
1155 static ssize_t sm501fb_crtsrc_store(struct device *dev,
1156                                 struct device_attribute *attr,
1157                                 const char *buf, size_t len)
1158 {
1159         struct sm501fb_info *info = dev_get_drvdata(dev);
1160         enum sm501_controller head;
1161         unsigned long ctrl;
1162
1163         if (len < 1)
1164                 return -EINVAL;
1165
1166         if (strnicmp(buf, "crt", 3) == 0)
1167                 head = HEAD_CRT;
1168         else if (strnicmp(buf, "panel", 5) == 0)
1169                 head = HEAD_PANEL;
1170         else
1171                 return -EINVAL;
1172
1173         dev_info(dev, "setting crt source to head %d\n", head);
1174
1175         ctrl = readl(info->regs + SM501_DC_CRT_CONTROL);
1176
1177         if (head == HEAD_CRT) {
1178                 ctrl |= SM501_DC_CRT_CONTROL_SEL;
1179                 ctrl |= SM501_DC_CRT_CONTROL_ENABLE;
1180                 ctrl |= SM501_DC_CRT_CONTROL_TE;
1181         } else {
1182                 ctrl &= ~SM501_DC_CRT_CONTROL_SEL;
1183                 ctrl &= ~SM501_DC_CRT_CONTROL_ENABLE;
1184                 ctrl &= ~SM501_DC_CRT_CONTROL_TE;
1185         }
1186
1187         writel(ctrl, info->regs + SM501_DC_CRT_CONTROL);
1188         sm501fb_sync_regs(info);
1189
1190         return len;
1191 }
1192
1193 /* Prepare the device_attr for registration with sysfs later */
1194 static DEVICE_ATTR(crt_src, 0666, sm501fb_crtsrc_show, sm501fb_crtsrc_store);
1195
1196 /* sm501fb_show_regs
1197  *
1198  * show the primary sm501 registers
1199 */
1200 static int sm501fb_show_regs(struct sm501fb_info *info, char *ptr,
1201                              unsigned int start, unsigned int len)
1202 {
1203         void __iomem *mem = info->regs;
1204         char *buf = ptr;
1205         unsigned int reg;
1206
1207         for (reg = start; reg < (len + start); reg += 4)
1208                 ptr += sprintf(ptr, "%08x = %08x\n", reg, readl(mem + reg));
1209
1210         return ptr - buf;
1211 }
1212
1213 /* sm501fb_debug_show_crt
1214  *
1215  * show the crt control and cursor registers
1216 */
1217
1218 static ssize_t sm501fb_debug_show_crt(struct device *dev,
1219                                   struct device_attribute *attr, char *buf)
1220 {
1221         struct sm501fb_info *info = dev_get_drvdata(dev);
1222         char *ptr = buf;
1223
1224         ptr += sm501fb_show_regs(info, ptr, SM501_DC_CRT_CONTROL, 0x40);
1225         ptr += sm501fb_show_regs(info, ptr, SM501_DC_CRT_HWC_BASE, 0x10);
1226
1227         return ptr - buf;
1228 }
1229
1230 static DEVICE_ATTR(fbregs_crt, 0444, sm501fb_debug_show_crt, NULL);
1231
1232 /* sm501fb_debug_show_pnl
1233  *
1234  * show the panel control and cursor registers
1235 */
1236
1237 static ssize_t sm501fb_debug_show_pnl(struct device *dev,
1238                                   struct device_attribute *attr, char *buf)
1239 {
1240         struct sm501fb_info *info = dev_get_drvdata(dev);
1241         char *ptr = buf;
1242
1243         ptr += sm501fb_show_regs(info, ptr, 0x0, 0x40);
1244         ptr += sm501fb_show_regs(info, ptr, SM501_DC_PANEL_HWC_BASE, 0x10);
1245
1246         return ptr - buf;
1247 }
1248
1249 static DEVICE_ATTR(fbregs_pnl, 0444, sm501fb_debug_show_pnl, NULL);
1250
1251 /* acceleration operations */
1252 static int sm501fb_sync(struct fb_info *info)
1253 {
1254         int count = 1000000;
1255         struct sm501fb_par  *par = info->par;
1256         struct sm501fb_info *fbi = par->info;
1257
1258         /* wait for the 2d engine to be ready */
1259         while ((count > 0) &&
1260                (readl(fbi->regs + SM501_SYSTEM_CONTROL) &
1261                 SM501_SYSCTRL_2D_ENGINE_STATUS) != 0)
1262                 count--;
1263
1264         if (count <= 0) {
1265                 dev_err(info->dev, "Timeout waiting for 2d engine sync\n");
1266                 return 1;
1267         }
1268         return 0;
1269 }
1270
1271 static void sm501fb_copyarea(struct fb_info *info, const struct fb_copyarea *area)
1272 {
1273         struct sm501fb_par  *par = info->par;
1274         struct sm501fb_info *fbi = par->info;
1275         int width = area->width;
1276         int height = area->height;
1277         int sx = area->sx;
1278         int sy = area->sy;
1279         int dx = area->dx;
1280         int dy = area->dy;
1281         unsigned long rtl = 0;
1282
1283         /* source clip */
1284         if ((sx >= info->var.xres_virtual) ||
1285             (sy >= info->var.yres_virtual))
1286                 /* source Area not within virtual screen, skipping */
1287                 return;
1288         if ((sx + width) >= info->var.xres_virtual)
1289                 width = info->var.xres_virtual - sx - 1;
1290         if ((sy + height) >= info->var.yres_virtual)
1291                 height = info->var.yres_virtual - sy - 1;
1292
1293         /* dest clip */
1294         if ((dx >= info->var.xres_virtual) ||
1295             (dy >= info->var.yres_virtual))
1296                 /* Destination Area not within virtual screen, skipping */
1297                 return;
1298         if ((dx + width) >= info->var.xres_virtual)
1299                 width = info->var.xres_virtual - dx - 1;
1300         if ((dy + height) >= info->var.yres_virtual)
1301                 height = info->var.yres_virtual - dy - 1;
1302
1303         if ((sx < dx) || (sy < dy)) {
1304                 rtl = 1 << 27;
1305                 sx += width - 1;
1306                 dx += width - 1;
1307                 sy += height - 1;
1308                 dy += height - 1;
1309         }
1310
1311         if (sm501fb_sync(info))
1312                 return;
1313
1314         /* set the base addresses */
1315         writel(par->screen.sm_addr, fbi->regs2d + SM501_2D_SOURCE_BASE);
1316         writel(par->screen.sm_addr, fbi->regs2d + SM501_2D_DESTINATION_BASE);
1317
1318         /* set the window width */
1319         writel((info->var.xres << 16) | info->var.xres,
1320                fbi->regs2d + SM501_2D_WINDOW_WIDTH);
1321
1322         /* set window stride */
1323         writel((info->var.xres_virtual << 16) | info->var.xres_virtual,
1324                fbi->regs2d + SM501_2D_PITCH);
1325
1326         /* set data format */
1327         switch (info->var.bits_per_pixel) {
1328         case 8:
1329                 writel(0, fbi->regs2d + SM501_2D_STRETCH);
1330                 break;
1331         case 16:
1332                 writel(0x00100000, fbi->regs2d + SM501_2D_STRETCH);
1333                 break;
1334         case 32:
1335                 writel(0x00200000, fbi->regs2d + SM501_2D_STRETCH);
1336                 break;
1337         }
1338
1339         /* 2d compare mask */
1340         writel(0xffffffff, fbi->regs2d + SM501_2D_COLOR_COMPARE_MASK);
1341
1342         /* 2d mask */
1343         writel(0xffffffff, fbi->regs2d + SM501_2D_MASK);
1344
1345         /* source and destination x y */
1346         writel((sx << 16) | sy, fbi->regs2d + SM501_2D_SOURCE);
1347         writel((dx << 16) | dy, fbi->regs2d + SM501_2D_DESTINATION);
1348
1349         /* w/h */
1350         writel((width << 16) | height, fbi->regs2d + SM501_2D_DIMENSION);
1351
1352         /* do area move */
1353         writel(0x800000cc | rtl, fbi->regs2d + SM501_2D_CONTROL);
1354 }
1355
1356 static void sm501fb_fillrect(struct fb_info *info, const struct fb_fillrect *rect)
1357 {
1358         struct sm501fb_par  *par = info->par;
1359         struct sm501fb_info *fbi = par->info;
1360         int width = rect->width, height = rect->height;
1361
1362         if ((rect->dx >= info->var.xres_virtual) ||
1363             (rect->dy >= info->var.yres_virtual))
1364                 /* Rectangle not within virtual screen, skipping */
1365                 return;
1366         if ((rect->dx + width) >= info->var.xres_virtual)
1367                 width = info->var.xres_virtual - rect->dx - 1;
1368         if ((rect->dy + height) >= info->var.yres_virtual)
1369                 height = info->var.yres_virtual - rect->dy - 1;
1370
1371         if (sm501fb_sync(info))
1372                 return;
1373
1374         /* set the base addresses */
1375         writel(par->screen.sm_addr, fbi->regs2d + SM501_2D_SOURCE_BASE);
1376         writel(par->screen.sm_addr, fbi->regs2d + SM501_2D_DESTINATION_BASE);
1377
1378         /* set the window width */
1379         writel((info->var.xres << 16) | info->var.xres,
1380                fbi->regs2d + SM501_2D_WINDOW_WIDTH);
1381
1382         /* set window stride */
1383         writel((info->var.xres_virtual << 16) | info->var.xres_virtual,
1384                fbi->regs2d + SM501_2D_PITCH);
1385
1386         /* set data format */
1387         switch (info->var.bits_per_pixel) {
1388         case 8:
1389                 writel(0, fbi->regs2d + SM501_2D_STRETCH);
1390                 break;
1391         case 16:
1392                 writel(0x00100000, fbi->regs2d + SM501_2D_STRETCH);
1393                 break;
1394         case 32:
1395                 writel(0x00200000, fbi->regs2d + SM501_2D_STRETCH);
1396                 break;
1397         }
1398
1399         /* 2d compare mask */
1400         writel(0xffffffff, fbi->regs2d + SM501_2D_COLOR_COMPARE_MASK);
1401
1402         /* 2d mask */
1403         writel(0xffffffff, fbi->regs2d + SM501_2D_MASK);
1404
1405         /* colour */
1406         writel(rect->color, fbi->regs2d + SM501_2D_FOREGROUND);
1407
1408         /* x y */
1409         writel((rect->dx << 16) | rect->dy, fbi->regs2d + SM501_2D_DESTINATION);
1410
1411         /* w/h */
1412         writel((width << 16) | height, fbi->regs2d + SM501_2D_DIMENSION);
1413
1414         /* do rectangle fill */
1415         writel(0x800100cc, fbi->regs2d + SM501_2D_CONTROL);
1416 }
1417
1418
1419 static struct fb_ops sm501fb_ops_crt = {
1420         .owner          = THIS_MODULE,
1421         .fb_check_var   = sm501fb_check_var_crt,
1422         .fb_set_par     = sm501fb_set_par_crt,
1423         .fb_blank       = sm501fb_blank_crt,
1424         .fb_setcolreg   = sm501fb_setcolreg,
1425         .fb_pan_display = sm501fb_pan_crt,
1426         .fb_cursor      = sm501fb_cursor,
1427         .fb_fillrect    = sm501fb_fillrect,
1428         .fb_copyarea    = sm501fb_copyarea,
1429         .fb_imageblit   = cfb_imageblit,
1430         .fb_sync        = sm501fb_sync,
1431 };
1432
1433 static struct fb_ops sm501fb_ops_pnl = {
1434         .owner          = THIS_MODULE,
1435         .fb_check_var   = sm501fb_check_var_pnl,
1436         .fb_set_par     = sm501fb_set_par_pnl,
1437         .fb_pan_display = sm501fb_pan_pnl,
1438         .fb_blank       = sm501fb_blank_pnl,
1439         .fb_setcolreg   = sm501fb_setcolreg,
1440         .fb_cursor      = sm501fb_cursor,
1441         .fb_fillrect    = sm501fb_fillrect,
1442         .fb_copyarea    = sm501fb_copyarea,
1443         .fb_imageblit   = cfb_imageblit,
1444         .fb_sync        = sm501fb_sync,
1445 };
1446
1447 /* sm501_init_cursor
1448  *
1449  * initialise hw cursor parameters
1450 */
1451
1452 static int sm501_init_cursor(struct fb_info *fbi, unsigned int reg_base)
1453 {
1454         struct sm501fb_par *par;
1455         struct sm501fb_info *info;
1456         int ret;
1457
1458         if (fbi == NULL)
1459                 return 0;
1460
1461         par = fbi->par;
1462         info = par->info;
1463
1464         par->cursor_regs = info->regs + reg_base;
1465
1466         ret = sm501_alloc_mem(info, &par->cursor, SM501_MEMF_CURSOR, 1024,
1467                               fbi->fix.smem_len);
1468         if (ret < 0)
1469                 return ret;
1470
1471         /* initialise the colour registers */
1472
1473         writel(par->cursor.sm_addr, par->cursor_regs + SM501_OFF_HWC_ADDR);
1474
1475         writel(0x00, par->cursor_regs + SM501_OFF_HWC_LOC);
1476         writel(0x00, par->cursor_regs + SM501_OFF_HWC_COLOR_1_2);
1477         writel(0x00, par->cursor_regs + SM501_OFF_HWC_COLOR_3);
1478         sm501fb_sync_regs(info);
1479
1480         return 0;
1481 }
1482
1483 /* sm501fb_info_start
1484  *
1485  * fills the par structure claiming resources and remapping etc.
1486 */
1487
1488 static int sm501fb_start(struct sm501fb_info *info,
1489                          struct platform_device *pdev)
1490 {
1491         struct resource *res;
1492         struct device *dev = &pdev->dev;
1493         int k;
1494         int ret;
1495
1496         info->irq = ret = platform_get_irq(pdev, 0);
1497         if (ret < 0) {
1498                 /* we currently do not use the IRQ */
1499                 dev_warn(dev, "no irq for device\n");
1500         }
1501
1502         /* allocate, reserve and remap resources for display
1503          * controller registers */
1504         res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
1505         if (res == NULL) {
1506                 dev_err(dev, "no resource definition for registers\n");
1507                 ret = -ENOENT;
1508                 goto err_release;
1509         }
1510
1511         info->regs_res = request_mem_region(res->start,
1512                                             resource_size(res),
1513                                             pdev->name);
1514
1515         if (info->regs_res == NULL) {
1516                 dev_err(dev, "cannot claim registers\n");
1517                 ret = -ENXIO;
1518                 goto err_release;
1519         }
1520
1521         info->regs = ioremap(res->start, resource_size(res));
1522         if (info->regs == NULL) {
1523                 dev_err(dev, "cannot remap registers\n");
1524                 ret = -ENXIO;
1525                 goto err_regs_res;
1526         }
1527
1528         /* allocate, reserve and remap resources for 2d
1529          * controller registers */
1530         res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
1531         if (res == NULL) {
1532                 dev_err(dev, "no resource definition for 2d registers\n");
1533                 ret = -ENOENT;
1534                 goto err_regs_map;
1535         }
1536
1537         info->regs2d_res = request_mem_region(res->start,
1538                                               resource_size(res),
1539                                               pdev->name);
1540
1541         if (info->regs2d_res == NULL) {
1542                 dev_err(dev, "cannot claim registers\n");
1543                 ret = -ENXIO;
1544                 goto err_regs_map;
1545         }
1546
1547         info->regs2d = ioremap(res->start, resource_size(res));
1548         if (info->regs2d == NULL) {
1549                 dev_err(dev, "cannot remap registers\n");
1550                 ret = -ENXIO;
1551                 goto err_regs2d_res;
1552         }
1553
1554         /* allocate, reserve resources for framebuffer */
1555         res = platform_get_resource(pdev, IORESOURCE_MEM, 2);
1556         if (res == NULL) {
1557                 dev_err(dev, "no memory resource defined\n");
1558                 ret = -ENXIO;
1559                 goto err_regs2d_map;
1560         }
1561
1562         info->fbmem_res = request_mem_region(res->start,
1563                                              resource_size(res),
1564                                              pdev->name);
1565         if (info->fbmem_res == NULL) {
1566                 dev_err(dev, "cannot claim framebuffer\n");
1567                 ret = -ENXIO;
1568                 goto err_regs2d_map;
1569         }
1570
1571         info->fbmem = ioremap(res->start, resource_size(res));
1572         if (info->fbmem == NULL) {
1573                 dev_err(dev, "cannot remap framebuffer\n");
1574                 goto err_mem_res;
1575         }
1576
1577         info->fbmem_len = resource_size(res);
1578
1579         /* clear framebuffer memory - avoids garbage data on unused fb */
1580         memset(info->fbmem, 0, info->fbmem_len);
1581
1582         /* clear palette ram - undefined at power on */
1583         for (k = 0; k < (256 * 3); k++)
1584                 writel(0, info->regs + SM501_DC_PANEL_PALETTE + (k * 4));
1585
1586         /* enable display controller */
1587         sm501_unit_power(dev->parent, SM501_GATE_DISPLAY, 1);
1588
1589         /* enable 2d controller */
1590         sm501_unit_power(dev->parent, SM501_GATE_2D_ENGINE, 1);
1591
1592         /* setup cursors */
1593         sm501_init_cursor(info->fb[HEAD_CRT], SM501_DC_CRT_HWC_ADDR);
1594         sm501_init_cursor(info->fb[HEAD_PANEL], SM501_DC_PANEL_HWC_ADDR);
1595
1596         return 0; /* everything is setup */
1597
1598  err_mem_res:
1599         release_resource(info->fbmem_res);
1600         kfree(info->fbmem_res);
1601
1602  err_regs2d_map:
1603         iounmap(info->regs2d);
1604
1605  err_regs2d_res:
1606         release_resource(info->regs2d_res);
1607         kfree(info->regs2d_res);
1608
1609  err_regs_map:
1610         iounmap(info->regs);
1611
1612  err_regs_res:
1613         release_resource(info->regs_res);
1614         kfree(info->regs_res);
1615
1616  err_release:
1617         return ret;
1618 }
1619
1620 static void sm501fb_stop(struct sm501fb_info *info)
1621 {
1622         /* disable display controller */
1623         sm501_unit_power(info->dev->parent, SM501_GATE_DISPLAY, 0);
1624
1625         iounmap(info->fbmem);
1626         release_resource(info->fbmem_res);
1627         kfree(info->fbmem_res);
1628
1629         iounmap(info->regs2d);
1630         release_resource(info->regs2d_res);
1631         kfree(info->regs2d_res);
1632
1633         iounmap(info->regs);
1634         release_resource(info->regs_res);
1635         kfree(info->regs_res);
1636 }
1637
1638 static int sm501fb_init_fb(struct fb_info *fb,
1639                            enum sm501_controller head,
1640                            const char *fbname)
1641 {
1642         struct sm501_platdata_fbsub *pd;
1643         struct sm501fb_par *par = fb->par;
1644         struct sm501fb_info *info = par->info;
1645         unsigned long ctrl;
1646         unsigned int enable;
1647         int ret;
1648
1649         switch (head) {
1650         case HEAD_CRT:
1651                 pd = info->pdata->fb_crt;
1652                 ctrl = readl(info->regs + SM501_DC_CRT_CONTROL);
1653                 enable = (ctrl & SM501_DC_CRT_CONTROL_ENABLE) ? 1 : 0;
1654
1655                 /* ensure we set the correct source register */
1656                 if (info->pdata->fb_route != SM501_FB_CRT_PANEL) {
1657                         ctrl |= SM501_DC_CRT_CONTROL_SEL;
1658                         writel(ctrl, info->regs + SM501_DC_CRT_CONTROL);
1659                 }
1660
1661                 break;
1662
1663         case HEAD_PANEL:
1664                 pd = info->pdata->fb_pnl;
1665                 ctrl = readl(info->regs + SM501_DC_PANEL_CONTROL);
1666                 enable = (ctrl & SM501_DC_PANEL_CONTROL_EN) ? 1 : 0;
1667                 break;
1668
1669         default:
1670                 pd = NULL;              /* stop compiler warnings */
1671                 ctrl = 0;
1672                 enable = 0;
1673                 BUG();
1674         }
1675
1676         dev_info(info->dev, "fb %s %sabled at start\n",
1677                  fbname, enable ? "en" : "dis");
1678
1679         /* check to see if our routing allows this */
1680
1681         if (head == HEAD_CRT && info->pdata->fb_route == SM501_FB_CRT_PANEL) {
1682                 ctrl &= ~SM501_DC_CRT_CONTROL_SEL;
1683                 writel(ctrl, info->regs + SM501_DC_CRT_CONTROL);
1684                 enable = 0;
1685         }
1686
1687         strlcpy(fb->fix.id, fbname, sizeof(fb->fix.id));
1688
1689         memcpy(&par->ops,
1690                (head == HEAD_CRT) ? &sm501fb_ops_crt : &sm501fb_ops_pnl,
1691                sizeof(struct fb_ops));
1692
1693         /* update ops dependant on what we've been passed */
1694
1695         if ((pd->flags & SM501FB_FLAG_USE_HWCURSOR) == 0)
1696                 par->ops.fb_cursor = NULL;
1697
1698         fb->fbops = &par->ops;
1699         fb->flags = FBINFO_FLAG_DEFAULT | FBINFO_READS_FAST |
1700                 FBINFO_HWACCEL_COPYAREA | FBINFO_HWACCEL_FILLRECT |
1701                 FBINFO_HWACCEL_XPAN | FBINFO_HWACCEL_YPAN;
1702
1703         /* fixed data */
1704
1705         fb->fix.type            = FB_TYPE_PACKED_PIXELS;
1706         fb->fix.type_aux        = 0;
1707         fb->fix.xpanstep        = 1;
1708         fb->fix.ypanstep        = 1;
1709         fb->fix.ywrapstep       = 0;
1710         fb->fix.accel           = FB_ACCEL_NONE;
1711
1712         /* screenmode */
1713
1714         fb->var.nonstd          = 0;
1715         fb->var.activate        = FB_ACTIVATE_NOW;
1716         fb->var.accel_flags     = 0;
1717         fb->var.vmode           = FB_VMODE_NONINTERLACED;
1718         fb->var.bits_per_pixel  = 16;
1719
1720         if (enable && (pd->flags & SM501FB_FLAG_USE_INIT_MODE) && 0) {
1721                 /* TODO read the mode from the current display */
1722
1723         } else {
1724                 if (pd->def_mode) {
1725                         dev_info(info->dev, "using supplied mode\n");
1726                         fb_videomode_to_var(&fb->var, pd->def_mode);
1727
1728                         fb->var.bits_per_pixel = pd->def_bpp ? pd->def_bpp : 8;
1729                         fb->var.xres_virtual = fb->var.xres;
1730                         fb->var.yres_virtual = fb->var.yres;
1731                 } else {
1732                         ret = fb_find_mode(&fb->var, fb,
1733                                            NULL, NULL, 0, NULL, 8);
1734
1735                         if (ret == 0 || ret == 4) {
1736                                 dev_err(info->dev,
1737                                         "failed to get initial mode\n");
1738                                 return -EINVAL;
1739                         }
1740                 }
1741         }
1742
1743         /* initialise and set the palette */
1744         if (fb_alloc_cmap(&fb->cmap, NR_PALETTE, 0)) {
1745                 dev_err(info->dev, "failed to allocate cmap memory\n");
1746                 return -ENOMEM;
1747         }
1748         fb_set_cmap(&fb->cmap, fb);
1749
1750         ret = (fb->fbops->fb_check_var)(&fb->var, fb);
1751         if (ret)
1752                 dev_err(info->dev, "check_var() failed on initial setup?\n");
1753
1754         return 0;
1755 }
1756
1757 /* default platform data if none is supplied (ie, PCI device) */
1758
1759 static struct sm501_platdata_fbsub sm501fb_pdata_crt = {
1760         .flags          = (SM501FB_FLAG_USE_INIT_MODE |
1761                            SM501FB_FLAG_USE_HWCURSOR |
1762                            SM501FB_FLAG_USE_HWACCEL |
1763                            SM501FB_FLAG_DISABLE_AT_EXIT),
1764
1765 };
1766
1767 static struct sm501_platdata_fbsub sm501fb_pdata_pnl = {
1768         .flags          = (SM501FB_FLAG_USE_INIT_MODE |
1769                            SM501FB_FLAG_USE_HWCURSOR |
1770                            SM501FB_FLAG_USE_HWACCEL |
1771                            SM501FB_FLAG_DISABLE_AT_EXIT),
1772 };
1773
1774 static struct sm501_platdata_fb sm501fb_def_pdata = {
1775         .fb_route               = SM501_FB_OWN,
1776         .fb_crt                 = &sm501fb_pdata_crt,
1777         .fb_pnl                 = &sm501fb_pdata_pnl,
1778 };
1779
1780 static char driver_name_crt[] = "sm501fb-crt";
1781 static char driver_name_pnl[] = "sm501fb-panel";
1782
1783 static int __devinit sm501fb_probe_one(struct sm501fb_info *info,
1784                                        enum sm501_controller head)
1785 {
1786         unsigned char *name = (head == HEAD_CRT) ? "crt" : "panel";
1787         struct sm501_platdata_fbsub *pd;
1788         struct sm501fb_par *par;
1789         struct fb_info *fbi;
1790
1791         pd = (head == HEAD_CRT) ? info->pdata->fb_crt : info->pdata->fb_pnl;
1792
1793         /* Do not initialise if we've not been given any platform data */
1794         if (pd == NULL) {
1795                 dev_info(info->dev, "no data for fb %s (disabled)\n", name);
1796                 return 0;
1797         }
1798
1799         fbi = framebuffer_alloc(sizeof(struct sm501fb_par), info->dev);
1800         if (fbi == NULL) {
1801                 dev_err(info->dev, "cannot allocate %s framebuffer\n", name);
1802                 return -ENOMEM;
1803         }
1804
1805         par = fbi->par;
1806         par->info = info;
1807         par->head = head;
1808         fbi->pseudo_palette = &par->pseudo_palette;
1809
1810         info->fb[head] = fbi;
1811
1812         return 0;
1813 }
1814
1815 /* Free up anything allocated by sm501fb_init_fb */
1816
1817 static void sm501_free_init_fb(struct sm501fb_info *info,
1818                                 enum sm501_controller head)
1819 {
1820         struct fb_info *fbi = info->fb[head];
1821
1822         fb_dealloc_cmap(&fbi->cmap);
1823 }
1824
1825 static int __devinit sm501fb_start_one(struct sm501fb_info *info,
1826                                        enum sm501_controller head,
1827                                        const char *drvname)
1828 {
1829         struct fb_info *fbi = info->fb[head];
1830         int ret;
1831
1832         if (!fbi)
1833                 return 0;
1834
1835         mutex_init(&info->fb[head]->mm_lock);
1836
1837         ret = sm501fb_init_fb(info->fb[head], head, drvname);
1838         if (ret) {
1839                 dev_err(info->dev, "cannot initialise fb %s\n", drvname);
1840                 return ret;
1841         }
1842
1843         ret = register_framebuffer(info->fb[head]);
1844         if (ret) {
1845                 dev_err(info->dev, "failed to register fb %s\n", drvname);
1846                 sm501_free_init_fb(info, head);
1847                 return ret;
1848         }
1849
1850         dev_info(info->dev, "fb%d: %s frame buffer\n", fbi->node, fbi->fix.id);
1851
1852         return 0;
1853 }
1854
1855 static int __devinit sm501fb_probe(struct platform_device *pdev)
1856 {
1857         struct sm501fb_info *info;
1858         struct device *dev = &pdev->dev;
1859         int ret;
1860
1861         /* allocate our framebuffers */
1862
1863         info = kzalloc(sizeof(struct sm501fb_info), GFP_KERNEL);
1864         if (!info) {
1865                 dev_err(dev, "failed to allocate state\n");
1866                 return -ENOMEM;
1867         }
1868
1869         info->dev = dev = &pdev->dev;
1870         platform_set_drvdata(pdev, info);
1871
1872         if (dev->parent->platform_data) {
1873                 struct sm501_platdata *pd = dev->parent->platform_data;
1874                 info->pdata = pd->fb;
1875         }
1876
1877         if (info->pdata == NULL) {
1878                 dev_info(dev, "using default configuration data\n");
1879                 info->pdata = &sm501fb_def_pdata;
1880         }
1881
1882         /* probe for the presence of each panel */
1883
1884         ret = sm501fb_probe_one(info, HEAD_CRT);
1885         if (ret < 0) {
1886                 dev_err(dev, "failed to probe CRT\n");
1887                 goto err_alloc;
1888         }
1889
1890         ret = sm501fb_probe_one(info, HEAD_PANEL);
1891         if (ret < 0) {
1892                 dev_err(dev, "failed to probe PANEL\n");
1893                 goto err_probed_crt;
1894         }
1895
1896         if (info->fb[HEAD_PANEL] == NULL &&
1897             info->fb[HEAD_CRT] == NULL) {
1898                 dev_err(dev, "no framebuffers found\n");
1899                 goto err_alloc;
1900         }
1901
1902         /* get the resources for both of the framebuffers */
1903
1904         ret = sm501fb_start(info, pdev);
1905         if (ret) {
1906                 dev_err(dev, "cannot initialise SM501\n");
1907                 goto err_probed_panel;
1908         }
1909
1910         ret = sm501fb_start_one(info, HEAD_CRT, driver_name_crt);
1911         if (ret) {
1912                 dev_err(dev, "failed to start CRT\n");
1913                 goto err_started;
1914         }
1915
1916         ret = sm501fb_start_one(info, HEAD_PANEL, driver_name_pnl);
1917         if (ret) {
1918                 dev_err(dev, "failed to start Panel\n");
1919                 goto err_started_crt;
1920         }
1921
1922         /* create device files */
1923
1924         ret = device_create_file(dev, &dev_attr_crt_src);
1925         if (ret)
1926                 goto err_started_panel;
1927
1928         ret = device_create_file(dev, &dev_attr_fbregs_pnl);
1929         if (ret)
1930                 goto err_attached_crtsrc_file;
1931
1932         ret = device_create_file(dev, &dev_attr_fbregs_crt);
1933         if (ret)
1934                 goto err_attached_pnlregs_file;
1935
1936         /* we registered, return ok */
1937         return 0;
1938
1939 err_attached_pnlregs_file:
1940         device_remove_file(dev, &dev_attr_fbregs_pnl);
1941
1942 err_attached_crtsrc_file:
1943         device_remove_file(dev, &dev_attr_crt_src);
1944
1945 err_started_panel:
1946         unregister_framebuffer(info->fb[HEAD_PANEL]);
1947         sm501_free_init_fb(info, HEAD_PANEL);
1948
1949 err_started_crt:
1950         unregister_framebuffer(info->fb[HEAD_CRT]);
1951         sm501_free_init_fb(info, HEAD_CRT);
1952
1953 err_started:
1954         sm501fb_stop(info);
1955
1956 err_probed_panel:
1957         framebuffer_release(info->fb[HEAD_PANEL]);
1958
1959 err_probed_crt:
1960         framebuffer_release(info->fb[HEAD_CRT]);
1961
1962 err_alloc:
1963         kfree(info);
1964
1965         return ret;
1966 }
1967
1968
1969 /*
1970  *  Cleanup
1971  */
1972 static int sm501fb_remove(struct platform_device *pdev)
1973 {
1974         struct sm501fb_info *info = platform_get_drvdata(pdev);
1975         struct fb_info     *fbinfo_crt = info->fb[0];
1976         struct fb_info     *fbinfo_pnl = info->fb[1];
1977
1978         device_remove_file(&pdev->dev, &dev_attr_fbregs_crt);
1979         device_remove_file(&pdev->dev, &dev_attr_fbregs_pnl);
1980         device_remove_file(&pdev->dev, &dev_attr_crt_src);
1981
1982         sm501_free_init_fb(info, HEAD_CRT);
1983         sm501_free_init_fb(info, HEAD_PANEL);
1984
1985         unregister_framebuffer(fbinfo_crt);
1986         unregister_framebuffer(fbinfo_pnl);
1987
1988         sm501fb_stop(info);
1989         kfree(info);
1990
1991         framebuffer_release(fbinfo_pnl);
1992         framebuffer_release(fbinfo_crt);
1993
1994         return 0;
1995 }
1996
1997 #ifdef CONFIG_PM
1998
1999 static int sm501fb_suspend_fb(struct sm501fb_info *info,
2000                               enum sm501_controller head)
2001 {
2002         struct fb_info *fbi = info->fb[head];
2003         struct sm501fb_par *par = fbi->par;
2004
2005         if (par->screen.size == 0)
2006                 return 0;
2007
2008         /* blank the relevant interface to ensure unit power minimised */
2009         (par->ops.fb_blank)(FB_BLANK_POWERDOWN, fbi);
2010
2011         /* tell console/fb driver we are suspending */
2012
2013         acquire_console_sem();
2014         fb_set_suspend(fbi, 1);
2015         release_console_sem();
2016
2017         /* backup copies in case chip is powered down over suspend */
2018
2019         par->store_fb = vmalloc(par->screen.size);
2020         if (par->store_fb == NULL) {
2021                 dev_err(info->dev, "no memory to store screen\n");
2022                 return -ENOMEM;
2023         }
2024
2025         par->store_cursor = vmalloc(par->cursor.size);
2026         if (par->store_cursor == NULL) {
2027                 dev_err(info->dev, "no memory to store cursor\n");
2028                 goto err_nocursor;
2029         }
2030
2031         dev_dbg(info->dev, "suspending screen to %p\n", par->store_fb);
2032         dev_dbg(info->dev, "suspending cursor to %p\n", par->store_cursor);
2033
2034         memcpy_fromio(par->store_fb, par->screen.k_addr, par->screen.size);
2035         memcpy_fromio(par->store_cursor, par->cursor.k_addr, par->cursor.size);
2036
2037         return 0;
2038
2039  err_nocursor:
2040         vfree(par->store_fb);
2041         par->store_fb = NULL;
2042
2043         return -ENOMEM;
2044 }
2045
2046 static void sm501fb_resume_fb(struct sm501fb_info *info,
2047                               enum sm501_controller head)
2048 {
2049         struct fb_info *fbi = info->fb[head];
2050         struct sm501fb_par *par = fbi->par;
2051
2052         if (par->screen.size == 0)
2053                 return;
2054
2055         /* re-activate the configuration */
2056
2057         (par->ops.fb_set_par)(fbi);
2058
2059         /* restore the data */
2060
2061         dev_dbg(info->dev, "restoring screen from %p\n", par->store_fb);
2062         dev_dbg(info->dev, "restoring cursor from %p\n", par->store_cursor);
2063
2064         if (par->store_fb)
2065                 memcpy_toio(par->screen.k_addr, par->store_fb,
2066                             par->screen.size);
2067
2068         if (par->store_cursor)
2069                 memcpy_toio(par->cursor.k_addr, par->store_cursor,
2070                             par->cursor.size);
2071
2072         acquire_console_sem();
2073         fb_set_suspend(fbi, 0);
2074         release_console_sem();
2075
2076         vfree(par->store_fb);
2077         vfree(par->store_cursor);
2078 }
2079
2080
2081 /* suspend and resume support */
2082
2083 static int sm501fb_suspend(struct platform_device *pdev, pm_message_t state)
2084 {
2085         struct sm501fb_info *info = platform_get_drvdata(pdev);
2086
2087         /* store crt control to resume with */
2088         info->pm_crt_ctrl = readl(info->regs + SM501_DC_CRT_CONTROL);
2089
2090         sm501fb_suspend_fb(info, HEAD_CRT);
2091         sm501fb_suspend_fb(info, HEAD_PANEL);
2092
2093         /* turn off the clocks, in case the device is not powered down */
2094         sm501_unit_power(info->dev->parent, SM501_GATE_DISPLAY, 0);
2095
2096         return 0;
2097 }
2098
2099 #define SM501_CRT_CTRL_SAVE (SM501_DC_CRT_CONTROL_TVP |        \
2100                              SM501_DC_CRT_CONTROL_SEL)
2101
2102
2103 static int sm501fb_resume(struct platform_device *pdev)
2104 {
2105         struct sm501fb_info *info = platform_get_drvdata(pdev);
2106         unsigned long crt_ctrl;
2107
2108         sm501_unit_power(info->dev->parent, SM501_GATE_DISPLAY, 1);
2109
2110         /* restore the items we want to be saved for crt control */
2111
2112         crt_ctrl = readl(info->regs + SM501_DC_CRT_CONTROL);
2113         crt_ctrl &= ~SM501_CRT_CTRL_SAVE;
2114         crt_ctrl |= info->pm_crt_ctrl & SM501_CRT_CTRL_SAVE;
2115         writel(crt_ctrl, info->regs + SM501_DC_CRT_CONTROL);
2116
2117         sm501fb_resume_fb(info, HEAD_CRT);
2118         sm501fb_resume_fb(info, HEAD_PANEL);
2119
2120         return 0;
2121 }
2122
2123 #else
2124 #define sm501fb_suspend NULL
2125 #define sm501fb_resume  NULL
2126 #endif
2127
2128 static struct platform_driver sm501fb_driver = {
2129         .probe          = sm501fb_probe,
2130         .remove         = sm501fb_remove,
2131         .suspend        = sm501fb_suspend,
2132         .resume         = sm501fb_resume,
2133         .driver         = {
2134                 .name   = "sm501-fb",
2135                 .owner  = THIS_MODULE,
2136         },
2137 };
2138
2139 static int __devinit sm501fb_init(void)
2140 {
2141         return platform_driver_register(&sm501fb_driver);
2142 }
2143
2144 static void __exit sm501fb_cleanup(void)
2145 {
2146         platform_driver_unregister(&sm501fb_driver);
2147 }
2148
2149 module_init(sm501fb_init);
2150 module_exit(sm501fb_cleanup);
2151
2152 MODULE_AUTHOR("Ben Dooks, Vincent Sanders");
2153 MODULE_DESCRIPTION("SM501 Framebuffer driver");
2154 MODULE_LICENSE("GPL v2");