Back to home page

OSCL-LXR
 
 

     

0001 // SPDX-License-Identifier: GPL-2.0-only
0002 /* linux/drivers/video/sm501fb.c
0003  *
0004  * Copyright (c) 2006 Simtec Electronics
0005  *  Vincent Sanders <vince@simtec.co.uk>
0006  *  Ben Dooks <ben@simtec.co.uk>
0007  *
0008  * Framebuffer driver for the Silicon Motion SM501
0009  */
0010 
0011 #include <linux/module.h>
0012 #include <linux/kernel.h>
0013 #include <linux/errno.h>
0014 #include <linux/string.h>
0015 #include <linux/mm.h>
0016 #include <linux/tty.h>
0017 #include <linux/slab.h>
0018 #include <linux/delay.h>
0019 #include <linux/fb.h>
0020 #include <linux/init.h>
0021 #include <linux/vmalloc.h>
0022 #include <linux/dma-mapping.h>
0023 #include <linux/interrupt.h>
0024 #include <linux/workqueue.h>
0025 #include <linux/wait.h>
0026 #include <linux/platform_device.h>
0027 #include <linux/clk.h>
0028 #include <linux/console.h>
0029 #include <linux/io.h>
0030 
0031 #include <linux/uaccess.h>
0032 #include <asm/div64.h>
0033 
0034 #ifdef CONFIG_PM
0035 #include <linux/pm.h>
0036 #endif
0037 
0038 #include <linux/sm501.h>
0039 #include <linux/sm501-regs.h>
0040 
0041 #include "edid.h"
0042 
0043 static char *fb_mode = "640x480-16@60";
0044 static unsigned long default_bpp = 16;
0045 
0046 static const struct fb_videomode sm501_default_mode = {
0047     .refresh    = 60,
0048     .xres       = 640,
0049     .yres       = 480,
0050     .pixclock   = 20833,
0051     .left_margin    = 142,
0052     .right_margin   = 13,
0053     .upper_margin   = 21,
0054     .lower_margin   = 1,
0055     .hsync_len  = 69,
0056     .vsync_len  = 3,
0057     .sync       = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
0058     .vmode      = FB_VMODE_NONINTERLACED
0059 };
0060 
0061 #define NR_PALETTE  256
0062 
0063 enum sm501_controller {
0064     HEAD_CRT    = 0,
0065     HEAD_PANEL  = 1,
0066 };
0067 
0068 /* SM501 memory address.
0069  *
0070  * This structure is used to track memory usage within the SM501 framebuffer
0071  * allocation. The sm_addr field is stored as an offset as it is often used
0072  * against both the physical and mapped addresses.
0073  */
0074 struct sm501_mem {
0075     unsigned long    size;
0076     unsigned long    sm_addr;   /* offset from base of sm501 fb. */
0077     void __iomem    *k_addr;
0078 };
0079 
0080 /* private data that is shared between all frambuffers* */
0081 struct sm501fb_info {
0082     struct device       *dev;
0083     struct fb_info      *fb[2];     /* fb info for both heads */
0084     struct resource     *fbmem_res; /* framebuffer resource */
0085     struct resource     *regs_res;  /* registers resource */
0086     struct resource     *regs2d_res;    /* 2d registers resource */
0087     struct sm501_platdata_fb *pdata;    /* our platform data */
0088 
0089     unsigned long        pm_crt_ctrl;   /* pm: crt ctrl save */
0090 
0091     int          irq;
0092     int          swap_endian;   /* set to swap rgb=>bgr */
0093     void __iomem        *regs;      /* remapped registers */
0094     void __iomem        *regs2d;    /* 2d remapped registers */
0095     void __iomem        *fbmem;     /* remapped framebuffer */
0096     size_t           fbmem_len; /* length of remapped region */
0097     u8 *edid_data;
0098 };
0099 
0100 /* per-framebuffer private data */
0101 struct sm501fb_par {
0102     u32          pseudo_palette[16];
0103 
0104     enum sm501_controller    head;
0105     struct sm501_mem     cursor;
0106     struct sm501_mem     screen;
0107     struct fb_ops        ops;
0108 
0109     void            *store_fb;
0110     void            *store_cursor;
0111     void __iomem        *cursor_regs;
0112     struct sm501fb_info *info;
0113 };
0114 
0115 /* Helper functions */
0116 
0117 static inline int h_total(struct fb_var_screeninfo *var)
0118 {
0119     return var->xres + var->left_margin +
0120         var->right_margin + var->hsync_len;
0121 }
0122 
0123 static inline int v_total(struct fb_var_screeninfo *var)
0124 {
0125     return var->yres + var->upper_margin +
0126         var->lower_margin + var->vsync_len;
0127 }
0128 
0129 /* sm501fb_sync_regs()
0130  *
0131  * This call is mainly for PCI bus systems where we need to
0132  * ensure that any writes to the bus are completed before the
0133  * next phase, or after completing a function.
0134 */
0135 
0136 static inline void sm501fb_sync_regs(struct sm501fb_info *info)
0137 {
0138     smc501_readl(info->regs);
0139 }
0140 
0141 /* sm501_alloc_mem
0142  *
0143  * This is an attempt to lay out memory for the two framebuffers and
0144  * everything else
0145  *
0146  * |fbmem_res->start                           fbmem_res->end|
0147  * |                                         |
0148  * |fb[0].fix.smem_start    |         |fb[1].fix.smem_start    |     2K      |
0149  * |-> fb[0].fix.smem_len <-| spare   |-> fb[1].fix.smem_len <-|-> cursors <-|
0150  *
0151  * The "spare" space is for the 2d engine data
0152  * the fixed is space for the cursors (2x1Kbyte)
0153  *
0154  * we need to allocate memory for the 2D acceleration engine
0155  * command list and the data for the engine to deal with.
0156  *
0157  * - all allocations must be 128bit aligned
0158  * - cursors are 64x64x2 bits (1Kbyte)
0159  *
0160  */
0161 
0162 #define SM501_MEMF_CURSOR       (1)
0163 #define SM501_MEMF_PANEL        (2)
0164 #define SM501_MEMF_CRT          (4)
0165 #define SM501_MEMF_ACCEL        (8)
0166 
0167 static int sm501_alloc_mem(struct sm501fb_info *inf, struct sm501_mem *mem,
0168                unsigned int why, size_t size, u32 smem_len)
0169 {
0170     struct sm501fb_par *par;
0171     struct fb_info *fbi;
0172     unsigned int ptr;
0173     unsigned int end;
0174 
0175     switch (why) {
0176     case SM501_MEMF_CURSOR:
0177         ptr = inf->fbmem_len - size;
0178         inf->fbmem_len = ptr;   /* adjust available memory. */
0179         break;
0180 
0181     case SM501_MEMF_PANEL:
0182         if (size > inf->fbmem_len)
0183             return -ENOMEM;
0184 
0185         ptr = inf->fbmem_len - size;
0186         fbi = inf->fb[HEAD_CRT];
0187 
0188         /* round down, some programs such as directfb do not draw
0189          * 0,0 correctly unless the start is aligned to a page start.
0190          */
0191 
0192         if (ptr > 0)
0193             ptr &= ~(PAGE_SIZE - 1);
0194 
0195         if (fbi && ptr < smem_len)
0196             return -ENOMEM;
0197 
0198         break;
0199 
0200     case SM501_MEMF_CRT:
0201         ptr = 0;
0202 
0203         /* check to see if we have panel memory allocated
0204          * which would put an limit on available memory. */
0205 
0206         fbi = inf->fb[HEAD_PANEL];
0207         if (fbi) {
0208             par = fbi->par;
0209             end = par->screen.k_addr ? par->screen.sm_addr : inf->fbmem_len;
0210         } else
0211             end = inf->fbmem_len;
0212 
0213         if ((ptr + size) > end)
0214             return -ENOMEM;
0215 
0216         break;
0217 
0218     case SM501_MEMF_ACCEL:
0219         fbi = inf->fb[HEAD_CRT];
0220         ptr = fbi ? smem_len : 0;
0221 
0222         fbi = inf->fb[HEAD_PANEL];
0223         if (fbi) {
0224             par = fbi->par;
0225             end = par->screen.sm_addr;
0226         } else
0227             end = inf->fbmem_len;
0228 
0229         if ((ptr + size) > end)
0230             return -ENOMEM;
0231 
0232         break;
0233 
0234     default:
0235         return -EINVAL;
0236     }
0237 
0238     mem->size    = size;
0239     mem->sm_addr = ptr;
0240     mem->k_addr  = inf->fbmem + ptr;
0241 
0242     dev_dbg(inf->dev, "%s: result %08lx, %p - %u, %zd\n",
0243         __func__, mem->sm_addr, mem->k_addr, why, size);
0244 
0245     return 0;
0246 }
0247 
0248 /* sm501fb_ps_to_hz
0249  *
0250  * Converts a period in picoseconds to Hz.
0251  *
0252  * Note, we try to keep this in Hz to minimise rounding with
0253  * the limited PLL settings on the SM501.
0254 */
0255 
0256 static unsigned long sm501fb_ps_to_hz(unsigned long psvalue)
0257 {
0258     unsigned long long numerator=1000000000000ULL;
0259 
0260     /* 10^12 / picosecond period gives frequency in Hz */
0261     do_div(numerator, psvalue);
0262     return (unsigned long)numerator;
0263 }
0264 
0265 /* sm501fb_hz_to_ps is identical to the opposite transform */
0266 
0267 #define sm501fb_hz_to_ps(x) sm501fb_ps_to_hz(x)
0268 
0269 /* sm501fb_setup_gamma
0270  *
0271  * Programs a linear 1.0 gamma ramp in case the gamma
0272  * correction is enabled without programming anything else.
0273 */
0274 
0275 static void sm501fb_setup_gamma(struct sm501fb_info *fbi,
0276                 unsigned long palette)
0277 {
0278     unsigned long value = 0;
0279     int offset;
0280 
0281     /* set gamma values */
0282     for (offset = 0; offset < 256 * 4; offset += 4) {
0283         smc501_writel(value, fbi->regs + palette + offset);
0284         value += 0x010101;  /* Advance RGB by 1,1,1.*/
0285     }
0286 }
0287 
0288 /* sm501fb_check_var
0289  *
0290  * check common variables for both panel and crt
0291 */
0292 
0293 static int sm501fb_check_var(struct fb_var_screeninfo *var,
0294                  struct fb_info *info)
0295 {
0296     struct sm501fb_par  *par = info->par;
0297     struct sm501fb_info *sm  = par->info;
0298     unsigned long tmp;
0299 
0300     /* check we can fit these values into the registers */
0301 
0302     if (var->hsync_len > 255 || var->vsync_len > 63)
0303         return -EINVAL;
0304 
0305     /* hdisplay end and hsync start */
0306     if ((var->xres + var->right_margin) > 4096)
0307         return -EINVAL;
0308 
0309     /* vdisplay end and vsync start */
0310     if ((var->yres + var->lower_margin) > 2048)
0311         return -EINVAL;
0312 
0313     /* hard limits of device */
0314 
0315     if (h_total(var) > 4096 || v_total(var) > 2048)
0316         return -EINVAL;
0317 
0318     /* check our line length is going to be 128 bit aligned */
0319 
0320     tmp = (var->xres * var->bits_per_pixel) / 8;
0321     if ((tmp & 15) != 0)
0322         return -EINVAL;
0323 
0324     /* check the virtual size */
0325 
0326     if (var->xres_virtual > 4096 || var->yres_virtual > 2048)
0327         return -EINVAL;
0328 
0329     /* can cope with 8,16 or 32bpp */
0330 
0331     if (var->bits_per_pixel <= 8)
0332         var->bits_per_pixel = 8;
0333     else if (var->bits_per_pixel <= 16)
0334         var->bits_per_pixel = 16;
0335     else if (var->bits_per_pixel == 24)
0336         var->bits_per_pixel = 32;
0337 
0338     /* set r/g/b positions and validate bpp */
0339     switch(var->bits_per_pixel) {
0340     case 8:
0341         var->red.length     = var->bits_per_pixel;
0342         var->red.offset     = 0;
0343         var->green.length   = var->bits_per_pixel;
0344         var->green.offset   = 0;
0345         var->blue.length    = var->bits_per_pixel;
0346         var->blue.offset    = 0;
0347         var->transp.length  = 0;
0348         var->transp.offset  = 0;
0349 
0350         break;
0351 
0352     case 16:
0353         if (sm->pdata->flags & SM501_FBPD_SWAP_FB_ENDIAN) {
0354             var->blue.offset    = 11;
0355             var->green.offset   = 5;
0356             var->red.offset     = 0;
0357         } else {
0358             var->red.offset     = 11;
0359             var->green.offset   = 5;
0360             var->blue.offset    = 0;
0361         }
0362         var->transp.offset  = 0;
0363 
0364         var->red.length     = 5;
0365         var->green.length   = 6;
0366         var->blue.length    = 5;
0367         var->transp.length  = 0;
0368         break;
0369 
0370     case 32:
0371         if (sm->pdata->flags & SM501_FBPD_SWAP_FB_ENDIAN) {
0372             var->transp.offset  = 0;
0373             var->red.offset     = 8;
0374             var->green.offset   = 16;
0375             var->blue.offset    = 24;
0376         } else {
0377             var->transp.offset  = 24;
0378             var->red.offset     = 16;
0379             var->green.offset   = 8;
0380             var->blue.offset    = 0;
0381         }
0382 
0383         var->red.length     = 8;
0384         var->green.length   = 8;
0385         var->blue.length    = 8;
0386         var->transp.length  = 0;
0387         break;
0388 
0389     default:
0390         return -EINVAL;
0391     }
0392 
0393     return 0;
0394 }
0395 
0396 /*
0397  * sm501fb_check_var_crt():
0398  *
0399  * check the parameters for the CRT head, and either bring them
0400  * back into range, or return -EINVAL.
0401 */
0402 
0403 static int sm501fb_check_var_crt(struct fb_var_screeninfo *var,
0404                  struct fb_info *info)
0405 {
0406     return sm501fb_check_var(var, info);
0407 }
0408 
0409 /* sm501fb_check_var_pnl():
0410  *
0411  * check the parameters for the CRT head, and either bring them
0412  * back into range, or return -EINVAL.
0413 */
0414 
0415 static int sm501fb_check_var_pnl(struct fb_var_screeninfo *var,
0416                  struct fb_info *info)
0417 {
0418     return sm501fb_check_var(var, info);
0419 }
0420 
0421 /* sm501fb_set_par_common
0422  *
0423  * set common registers for framebuffers
0424 */
0425 
0426 static int sm501fb_set_par_common(struct fb_info *info,
0427                   struct fb_var_screeninfo *var)
0428 {
0429     struct sm501fb_par  *par = info->par;
0430     struct sm501fb_info *fbi = par->info;
0431     unsigned long pixclock;      /* pixelclock in Hz */
0432     unsigned long sm501pixclock; /* pixelclock the 501 can achieve in Hz */
0433     unsigned int mem_type;
0434     unsigned int clock_type;
0435     unsigned int head_addr;
0436     unsigned int smem_len;
0437 
0438     dev_dbg(fbi->dev, "%s: %dx%d, bpp = %d, virtual %dx%d\n",
0439         __func__, var->xres, var->yres, var->bits_per_pixel,
0440         var->xres_virtual, var->yres_virtual);
0441 
0442     switch (par->head) {
0443     case HEAD_CRT:
0444         mem_type = SM501_MEMF_CRT;
0445         clock_type = SM501_CLOCK_V2XCLK;
0446         head_addr = SM501_DC_CRT_FB_ADDR;
0447         break;
0448 
0449     case HEAD_PANEL:
0450         mem_type = SM501_MEMF_PANEL;
0451         clock_type = SM501_CLOCK_P2XCLK;
0452         head_addr = SM501_DC_PANEL_FB_ADDR;
0453         break;
0454 
0455     default:
0456         mem_type = 0;       /* stop compiler warnings */
0457         head_addr = 0;
0458         clock_type = 0;
0459     }
0460 
0461     switch (var->bits_per_pixel) {
0462     case 8:
0463         info->fix.visual = FB_VISUAL_PSEUDOCOLOR;
0464         break;
0465 
0466     case 16:
0467         info->fix.visual = FB_VISUAL_TRUECOLOR;
0468         break;
0469 
0470     case 32:
0471         info->fix.visual = FB_VISUAL_TRUECOLOR;
0472         break;
0473     }
0474 
0475     /* allocate fb memory within 501 */
0476     info->fix.line_length = (var->xres_virtual * var->bits_per_pixel)/8;
0477     smem_len = info->fix.line_length * var->yres_virtual;
0478 
0479     dev_dbg(fbi->dev, "%s: line length = %u\n", __func__,
0480         info->fix.line_length);
0481 
0482     if (sm501_alloc_mem(fbi, &par->screen, mem_type, smem_len, smem_len)) {
0483         dev_err(fbi->dev, "no memory available\n");
0484         return -ENOMEM;
0485     }
0486 
0487     mutex_lock(&info->mm_lock);
0488     info->fix.smem_start = fbi->fbmem_res->start + par->screen.sm_addr;
0489     info->fix.smem_len   = smem_len;
0490     mutex_unlock(&info->mm_lock);
0491 
0492     info->screen_base = fbi->fbmem + par->screen.sm_addr;
0493     info->screen_size = info->fix.smem_len;
0494 
0495     /* set start of framebuffer to the screen */
0496 
0497     smc501_writel(par->screen.sm_addr | SM501_ADDR_FLIP,
0498             fbi->regs + head_addr);
0499 
0500     /* program CRT clock  */
0501 
0502     pixclock = sm501fb_ps_to_hz(var->pixclock);
0503 
0504     sm501pixclock = sm501_set_clock(fbi->dev->parent, clock_type,
0505                     pixclock);
0506 
0507     /* update fb layer with actual clock used */
0508     var->pixclock = sm501fb_hz_to_ps(sm501pixclock);
0509 
0510     dev_dbg(fbi->dev, "%s: pixclock(ps) = %u, pixclock(Hz)  = %lu, "
0511            "sm501pixclock = %lu,  error = %ld%%\n",
0512            __func__, var->pixclock, pixclock, sm501pixclock,
0513            ((pixclock - sm501pixclock)*100)/pixclock);
0514 
0515     return 0;
0516 }
0517 
0518 /* sm501fb_set_par_geometry
0519  *
0520  * set the geometry registers for specified framebuffer.
0521 */
0522 
0523 static void sm501fb_set_par_geometry(struct fb_info *info,
0524                      struct fb_var_screeninfo *var)
0525 {
0526     struct sm501fb_par  *par = info->par;
0527     struct sm501fb_info *fbi = par->info;
0528     void __iomem *base = fbi->regs;
0529     unsigned long reg;
0530 
0531     if (par->head == HEAD_CRT)
0532         base += SM501_DC_CRT_H_TOT;
0533     else
0534         base += SM501_DC_PANEL_H_TOT;
0535 
0536     /* set framebuffer width and display width */
0537 
0538     reg = info->fix.line_length;
0539     reg |= ((var->xres * var->bits_per_pixel)/8) << 16;
0540 
0541     smc501_writel(reg, fbi->regs + (par->head == HEAD_CRT ?
0542             SM501_DC_CRT_FB_OFFSET :  SM501_DC_PANEL_FB_OFFSET));
0543 
0544     /* program horizontal total */
0545 
0546     reg  = (h_total(var) - 1) << 16;
0547     reg |= (var->xres - 1);
0548 
0549     smc501_writel(reg, base + SM501_OFF_DC_H_TOT);
0550 
0551     /* program horizontal sync */
0552 
0553     reg  = var->hsync_len << 16;
0554     reg |= var->xres + var->right_margin - 1;
0555 
0556     smc501_writel(reg, base + SM501_OFF_DC_H_SYNC);
0557 
0558     /* program vertical total */
0559 
0560     reg  = (v_total(var) - 1) << 16;
0561     reg |= (var->yres - 1);
0562 
0563     smc501_writel(reg, base + SM501_OFF_DC_V_TOT);
0564 
0565     /* program vertical sync */
0566     reg  = var->vsync_len << 16;
0567     reg |= var->yres + var->lower_margin - 1;
0568 
0569     smc501_writel(reg, base + SM501_OFF_DC_V_SYNC);
0570 }
0571 
0572 /* sm501fb_pan_crt
0573  *
0574  * pan the CRT display output within an virtual framebuffer
0575 */
0576 
0577 static int sm501fb_pan_crt(struct fb_var_screeninfo *var,
0578                struct fb_info *info)
0579 {
0580     struct sm501fb_par  *par = info->par;
0581     struct sm501fb_info *fbi = par->info;
0582     unsigned int bytes_pixel = info->var.bits_per_pixel / 8;
0583     unsigned long reg;
0584     unsigned long xoffs;
0585 
0586     xoffs = var->xoffset * bytes_pixel;
0587 
0588     reg = smc501_readl(fbi->regs + SM501_DC_CRT_CONTROL);
0589 
0590     reg &= ~SM501_DC_CRT_CONTROL_PIXEL_MASK;
0591     reg |= ((xoffs & 15) / bytes_pixel) << 4;
0592     smc501_writel(reg, fbi->regs + SM501_DC_CRT_CONTROL);
0593 
0594     reg = (par->screen.sm_addr + xoffs +
0595            var->yoffset * info->fix.line_length);
0596     smc501_writel(reg | SM501_ADDR_FLIP, fbi->regs + SM501_DC_CRT_FB_ADDR);
0597 
0598     sm501fb_sync_regs(fbi);
0599     return 0;
0600 }
0601 
0602 /* sm501fb_pan_pnl
0603  *
0604  * pan the panel display output within an virtual framebuffer
0605 */
0606 
0607 static int sm501fb_pan_pnl(struct fb_var_screeninfo *var,
0608                struct fb_info *info)
0609 {
0610     struct sm501fb_par  *par = info->par;
0611     struct sm501fb_info *fbi = par->info;
0612     unsigned long reg;
0613 
0614     reg = var->xoffset | (info->var.xres_virtual << 16);
0615     smc501_writel(reg, fbi->regs + SM501_DC_PANEL_FB_WIDTH);
0616 
0617     reg = var->yoffset | (info->var.yres_virtual << 16);
0618     smc501_writel(reg, fbi->regs + SM501_DC_PANEL_FB_HEIGHT);
0619 
0620     sm501fb_sync_regs(fbi);
0621     return 0;
0622 }
0623 
0624 /* sm501fb_set_par_crt
0625  *
0626  * Set the CRT video mode from the fb_info structure
0627 */
0628 
0629 static int sm501fb_set_par_crt(struct fb_info *info)
0630 {
0631     struct sm501fb_par  *par = info->par;
0632     struct sm501fb_info *fbi = par->info;
0633     struct fb_var_screeninfo *var = &info->var;
0634     unsigned long control;       /* control register */
0635     int ret;
0636 
0637     /* activate new configuration */
0638 
0639     dev_dbg(fbi->dev, "%s(%p)\n", __func__, info);
0640 
0641     /* enable CRT DAC - note 0 is on!*/
0642     sm501_misc_control(fbi->dev->parent, 0, SM501_MISC_DAC_POWER);
0643 
0644     control = smc501_readl(fbi->regs + SM501_DC_CRT_CONTROL);
0645 
0646     control &= (SM501_DC_CRT_CONTROL_PIXEL_MASK |
0647             SM501_DC_CRT_CONTROL_GAMMA |
0648             SM501_DC_CRT_CONTROL_BLANK |
0649             SM501_DC_CRT_CONTROL_SEL |
0650             SM501_DC_CRT_CONTROL_CP |
0651             SM501_DC_CRT_CONTROL_TVP);
0652 
0653     /* set the sync polarities before we check data source  */
0654 
0655     if ((var->sync & FB_SYNC_HOR_HIGH_ACT) == 0)
0656         control |= SM501_DC_CRT_CONTROL_HSP;
0657 
0658     if ((var->sync & FB_SYNC_VERT_HIGH_ACT) == 0)
0659         control |= SM501_DC_CRT_CONTROL_VSP;
0660 
0661     if ((control & SM501_DC_CRT_CONTROL_SEL) == 0) {
0662         /* the head is displaying panel data... */
0663 
0664         sm501_alloc_mem(fbi, &par->screen, SM501_MEMF_CRT, 0,
0665                 info->fix.smem_len);
0666         goto out_update;
0667     }
0668 
0669     ret = sm501fb_set_par_common(info, var);
0670     if (ret) {
0671         dev_err(fbi->dev, "failed to set common parameters\n");
0672         return ret;
0673     }
0674 
0675     sm501fb_pan_crt(var, info);
0676     sm501fb_set_par_geometry(info, var);
0677 
0678     control |= SM501_FIFO_3;    /* fill if >3 free slots */
0679 
0680     switch(var->bits_per_pixel) {
0681     case 8:
0682         control |= SM501_DC_CRT_CONTROL_8BPP;
0683         break;
0684 
0685     case 16:
0686         control |= SM501_DC_CRT_CONTROL_16BPP;
0687         sm501fb_setup_gamma(fbi, SM501_DC_CRT_PALETTE);
0688         break;
0689 
0690     case 32:
0691         control |= SM501_DC_CRT_CONTROL_32BPP;
0692         sm501fb_setup_gamma(fbi, SM501_DC_CRT_PALETTE);
0693         break;
0694 
0695     default:
0696         BUG();
0697     }
0698 
0699     control |= SM501_DC_CRT_CONTROL_SEL;    /* CRT displays CRT data */
0700     control |= SM501_DC_CRT_CONTROL_TE; /* enable CRT timing */
0701     control |= SM501_DC_CRT_CONTROL_ENABLE; /* enable CRT plane */
0702 
0703  out_update:
0704     dev_dbg(fbi->dev, "new control is %08lx\n", control);
0705 
0706     smc501_writel(control, fbi->regs + SM501_DC_CRT_CONTROL);
0707     sm501fb_sync_regs(fbi);
0708 
0709     return 0;
0710 }
0711 
0712 static void sm501fb_panel_power(struct sm501fb_info *fbi, int to)
0713 {
0714     unsigned long control;
0715     void __iomem *ctrl_reg = fbi->regs + SM501_DC_PANEL_CONTROL;
0716     struct sm501_platdata_fbsub *pd = fbi->pdata->fb_pnl;
0717 
0718     control = smc501_readl(ctrl_reg);
0719 
0720     if (to && (control & SM501_DC_PANEL_CONTROL_VDD) == 0) {
0721         /* enable panel power */
0722 
0723         control |= SM501_DC_PANEL_CONTROL_VDD;  /* FPVDDEN */
0724         smc501_writel(control, ctrl_reg);
0725         sm501fb_sync_regs(fbi);
0726         mdelay(10);
0727 
0728         control |= SM501_DC_PANEL_CONTROL_DATA; /* DATA */
0729         smc501_writel(control, ctrl_reg);
0730         sm501fb_sync_regs(fbi);
0731         mdelay(10);
0732 
0733         /* VBIASEN */
0734 
0735         if (!(pd->flags & SM501FB_FLAG_PANEL_NO_VBIASEN)) {
0736             if (pd->flags & SM501FB_FLAG_PANEL_INV_VBIASEN)
0737                 control &= ~SM501_DC_PANEL_CONTROL_BIAS;
0738             else
0739                 control |= SM501_DC_PANEL_CONTROL_BIAS;
0740 
0741             smc501_writel(control, ctrl_reg);
0742             sm501fb_sync_regs(fbi);
0743             mdelay(10);
0744         }
0745 
0746         if (!(pd->flags & SM501FB_FLAG_PANEL_NO_FPEN)) {
0747             if (pd->flags & SM501FB_FLAG_PANEL_INV_FPEN)
0748                 control &= ~SM501_DC_PANEL_CONTROL_FPEN;
0749             else
0750                 control |= SM501_DC_PANEL_CONTROL_FPEN;
0751 
0752             smc501_writel(control, ctrl_reg);
0753             sm501fb_sync_regs(fbi);
0754             mdelay(10);
0755         }
0756     } else if (!to && (control & SM501_DC_PANEL_CONTROL_VDD) != 0) {
0757         /* disable panel power */
0758         if (!(pd->flags & SM501FB_FLAG_PANEL_NO_FPEN)) {
0759             if (pd->flags & SM501FB_FLAG_PANEL_INV_FPEN)
0760                 control |= SM501_DC_PANEL_CONTROL_FPEN;
0761             else
0762                 control &= ~SM501_DC_PANEL_CONTROL_FPEN;
0763 
0764             smc501_writel(control, ctrl_reg);
0765             sm501fb_sync_regs(fbi);
0766             mdelay(10);
0767         }
0768 
0769         if (!(pd->flags & SM501FB_FLAG_PANEL_NO_VBIASEN)) {
0770             if (pd->flags & SM501FB_FLAG_PANEL_INV_VBIASEN)
0771                 control |= SM501_DC_PANEL_CONTROL_BIAS;
0772             else
0773                 control &= ~SM501_DC_PANEL_CONTROL_BIAS;
0774 
0775             smc501_writel(control, ctrl_reg);
0776             sm501fb_sync_regs(fbi);
0777             mdelay(10);
0778         }
0779 
0780         control &= ~SM501_DC_PANEL_CONTROL_DATA;
0781         smc501_writel(control, ctrl_reg);
0782         sm501fb_sync_regs(fbi);
0783         mdelay(10);
0784 
0785         control &= ~SM501_DC_PANEL_CONTROL_VDD;
0786         smc501_writel(control, ctrl_reg);
0787         sm501fb_sync_regs(fbi);
0788         mdelay(10);
0789     }
0790 
0791     sm501fb_sync_regs(fbi);
0792 }
0793 
0794 /* sm501fb_set_par_pnl
0795  *
0796  * Set the panel video mode from the fb_info structure
0797 */
0798 
0799 static int sm501fb_set_par_pnl(struct fb_info *info)
0800 {
0801     struct sm501fb_par  *par = info->par;
0802     struct sm501fb_info *fbi = par->info;
0803     struct fb_var_screeninfo *var = &info->var;
0804     unsigned long control;
0805     unsigned long reg;
0806     int ret;
0807 
0808     dev_dbg(fbi->dev, "%s(%p)\n", __func__, info);
0809 
0810     /* activate this new configuration */
0811 
0812     ret = sm501fb_set_par_common(info, var);
0813     if (ret)
0814         return ret;
0815 
0816     sm501fb_pan_pnl(var, info);
0817     sm501fb_set_par_geometry(info, var);
0818 
0819     /* update control register */
0820 
0821     control = smc501_readl(fbi->regs + SM501_DC_PANEL_CONTROL);
0822     control &= (SM501_DC_PANEL_CONTROL_GAMMA |
0823             SM501_DC_PANEL_CONTROL_VDD  |
0824             SM501_DC_PANEL_CONTROL_DATA |
0825             SM501_DC_PANEL_CONTROL_BIAS |
0826             SM501_DC_PANEL_CONTROL_FPEN |
0827             SM501_DC_PANEL_CONTROL_CP |
0828             SM501_DC_PANEL_CONTROL_CK |
0829             SM501_DC_PANEL_CONTROL_HP |
0830             SM501_DC_PANEL_CONTROL_VP |
0831             SM501_DC_PANEL_CONTROL_HPD |
0832             SM501_DC_PANEL_CONTROL_VPD);
0833 
0834     control |= SM501_FIFO_3;    /* fill if >3 free slots */
0835 
0836     switch(var->bits_per_pixel) {
0837     case 8:
0838         control |= SM501_DC_PANEL_CONTROL_8BPP;
0839         break;
0840 
0841     case 16:
0842         control |= SM501_DC_PANEL_CONTROL_16BPP;
0843         sm501fb_setup_gamma(fbi, SM501_DC_PANEL_PALETTE);
0844         break;
0845 
0846     case 32:
0847         control |= SM501_DC_PANEL_CONTROL_32BPP;
0848         sm501fb_setup_gamma(fbi, SM501_DC_PANEL_PALETTE);
0849         break;
0850 
0851     default:
0852         BUG();
0853     }
0854 
0855     smc501_writel(0x0, fbi->regs + SM501_DC_PANEL_PANNING_CONTROL);
0856 
0857     /* panel plane top left and bottom right location */
0858 
0859     smc501_writel(0x00, fbi->regs + SM501_DC_PANEL_TL_LOC);
0860 
0861     reg  = var->xres - 1;
0862     reg |= (var->yres - 1) << 16;
0863 
0864     smc501_writel(reg, fbi->regs + SM501_DC_PANEL_BR_LOC);
0865 
0866     /* program panel control register */
0867 
0868     control |= SM501_DC_PANEL_CONTROL_TE;   /* enable PANEL timing */
0869     control |= SM501_DC_PANEL_CONTROL_EN;   /* enable PANEL gfx plane */
0870 
0871     if ((var->sync & FB_SYNC_HOR_HIGH_ACT) == 0)
0872         control |= SM501_DC_PANEL_CONTROL_HSP;
0873 
0874     if ((var->sync & FB_SYNC_VERT_HIGH_ACT) == 0)
0875         control |= SM501_DC_PANEL_CONTROL_VSP;
0876 
0877     smc501_writel(control, fbi->regs + SM501_DC_PANEL_CONTROL);
0878     sm501fb_sync_regs(fbi);
0879 
0880     /* ensure the panel interface is not tristated at this point */
0881 
0882     sm501_modify_reg(fbi->dev->parent, SM501_SYSTEM_CONTROL,
0883              0, SM501_SYSCTRL_PANEL_TRISTATE);
0884 
0885     /* power the panel up */
0886     sm501fb_panel_power(fbi, 1);
0887     return 0;
0888 }
0889 
0890 
0891 /* chan_to_field
0892  *
0893  * convert a colour value into a field position
0894  *
0895  * from pxafb.c
0896 */
0897 
0898 static inline unsigned int chan_to_field(unsigned int chan,
0899                      struct fb_bitfield *bf)
0900 {
0901     chan &= 0xffff;
0902     chan >>= 16 - bf->length;
0903     return chan << bf->offset;
0904 }
0905 
0906 /* sm501fb_setcolreg
0907  *
0908  * set the colour mapping for modes that support palettised data
0909 */
0910 
0911 static int sm501fb_setcolreg(unsigned regno,
0912                  unsigned red, unsigned green, unsigned blue,
0913                  unsigned transp, struct fb_info *info)
0914 {
0915     struct sm501fb_par  *par = info->par;
0916     struct sm501fb_info *fbi = par->info;
0917     void __iomem *base = fbi->regs;
0918     unsigned int val;
0919 
0920     if (par->head == HEAD_CRT)
0921         base += SM501_DC_CRT_PALETTE;
0922     else
0923         base += SM501_DC_PANEL_PALETTE;
0924 
0925     switch (info->fix.visual) {
0926     case FB_VISUAL_TRUECOLOR:
0927         /* true-colour, use pseuo-palette */
0928 
0929         if (regno < 16) {
0930             u32 *pal = par->pseudo_palette;
0931 
0932             val  = chan_to_field(red,   &info->var.red);
0933             val |= chan_to_field(green, &info->var.green);
0934             val |= chan_to_field(blue,  &info->var.blue);
0935 
0936             pal[regno] = val;
0937         }
0938         break;
0939 
0940     case FB_VISUAL_PSEUDOCOLOR:
0941         if (regno < 256) {
0942             val = (red >> 8) << 16;
0943             val |= (green >> 8) << 8;
0944             val |= blue >> 8;
0945 
0946             smc501_writel(val, base + (regno * 4));
0947         }
0948 
0949         break;
0950 
0951     default:
0952         return 1;   /* unknown type */
0953     }
0954 
0955     return 0;
0956 }
0957 
0958 /* sm501fb_blank_pnl
0959  *
0960  * Blank or un-blank the panel interface
0961 */
0962 
0963 static int sm501fb_blank_pnl(int blank_mode, struct fb_info *info)
0964 {
0965     struct sm501fb_par  *par = info->par;
0966     struct sm501fb_info *fbi = par->info;
0967 
0968     dev_dbg(fbi->dev, "%s(mode=%d, %p)\n", __func__, blank_mode, info);
0969 
0970     switch (blank_mode) {
0971     case FB_BLANK_POWERDOWN:
0972         sm501fb_panel_power(fbi, 0);
0973         break;
0974 
0975     case FB_BLANK_UNBLANK:
0976         sm501fb_panel_power(fbi, 1);
0977         break;
0978 
0979     case FB_BLANK_NORMAL:
0980     case FB_BLANK_VSYNC_SUSPEND:
0981     case FB_BLANK_HSYNC_SUSPEND:
0982     default:
0983         return 1;
0984     }
0985 
0986     return 0;
0987 }
0988 
0989 /* sm501fb_blank_crt
0990  *
0991  * Blank or un-blank the crt interface
0992 */
0993 
0994 static int sm501fb_blank_crt(int blank_mode, struct fb_info *info)
0995 {
0996     struct sm501fb_par  *par = info->par;
0997     struct sm501fb_info *fbi = par->info;
0998     unsigned long ctrl;
0999 
1000     dev_dbg(fbi->dev, "%s(mode=%d, %p)\n", __func__, blank_mode, info);
1001 
1002     ctrl = smc501_readl(fbi->regs + SM501_DC_CRT_CONTROL);
1003 
1004     switch (blank_mode) {
1005     case FB_BLANK_POWERDOWN:
1006         ctrl &= ~SM501_DC_CRT_CONTROL_ENABLE;
1007         sm501_misc_control(fbi->dev->parent, SM501_MISC_DAC_POWER, 0);
1008         fallthrough;
1009 
1010     case FB_BLANK_NORMAL:
1011         ctrl |= SM501_DC_CRT_CONTROL_BLANK;
1012         break;
1013 
1014     case FB_BLANK_UNBLANK:
1015         ctrl &= ~SM501_DC_CRT_CONTROL_BLANK;
1016         ctrl |=  SM501_DC_CRT_CONTROL_ENABLE;
1017         sm501_misc_control(fbi->dev->parent, 0, SM501_MISC_DAC_POWER);
1018         break;
1019 
1020     case FB_BLANK_VSYNC_SUSPEND:
1021     case FB_BLANK_HSYNC_SUSPEND:
1022     default:
1023         return 1;
1024 
1025     }
1026 
1027     smc501_writel(ctrl, fbi->regs + SM501_DC_CRT_CONTROL);
1028     sm501fb_sync_regs(fbi);
1029 
1030     return 0;
1031 }
1032 
1033 /* sm501fb_cursor
1034  *
1035  * set or change the hardware cursor parameters
1036 */
1037 
1038 static int sm501fb_cursor(struct fb_info *info, struct fb_cursor *cursor)
1039 {
1040     struct sm501fb_par  *par = info->par;
1041     struct sm501fb_info *fbi = par->info;
1042     void __iomem *base = fbi->regs;
1043     unsigned long hwc_addr;
1044     unsigned long fg, bg;
1045 
1046     dev_dbg(fbi->dev, "%s(%p,%p)\n", __func__, info, cursor);
1047 
1048     if (par->head == HEAD_CRT)
1049         base += SM501_DC_CRT_HWC_BASE;
1050     else
1051         base += SM501_DC_PANEL_HWC_BASE;
1052 
1053     /* check not being asked to exceed capabilities */
1054 
1055     if (cursor->image.width > 64)
1056         return -EINVAL;
1057 
1058     if (cursor->image.height > 64)
1059         return -EINVAL;
1060 
1061     if (cursor->image.depth > 1)
1062         return -EINVAL;
1063 
1064     hwc_addr = smc501_readl(base + SM501_OFF_HWC_ADDR);
1065 
1066     if (cursor->enable)
1067         smc501_writel(hwc_addr | SM501_HWC_EN,
1068                 base + SM501_OFF_HWC_ADDR);
1069     else
1070         smc501_writel(hwc_addr & ~SM501_HWC_EN,
1071                 base + SM501_OFF_HWC_ADDR);
1072 
1073     /* set data */
1074     if (cursor->set & FB_CUR_SETPOS) {
1075         unsigned int x = cursor->image.dx;
1076         unsigned int y = cursor->image.dy;
1077 
1078         if (x >= 2048 || y >= 2048 )
1079             return -EINVAL;
1080 
1081         dev_dbg(fbi->dev, "set position %d,%d\n", x, y);
1082 
1083         //y += cursor->image.height;
1084 
1085         smc501_writel(x | (y << 16), base + SM501_OFF_HWC_LOC);
1086     }
1087 
1088     if (cursor->set & FB_CUR_SETCMAP) {
1089         unsigned int bg_col = cursor->image.bg_color;
1090         unsigned int fg_col = cursor->image.fg_color;
1091 
1092         dev_dbg(fbi->dev, "%s: update cmap (%08x,%08x)\n",
1093             __func__, bg_col, fg_col);
1094 
1095         bg = ((info->cmap.red[bg_col] & 0xF8) << 8) |
1096             ((info->cmap.green[bg_col] & 0xFC) << 3) |
1097             ((info->cmap.blue[bg_col] & 0xF8) >> 3);
1098 
1099         fg = ((info->cmap.red[fg_col] & 0xF8) << 8) |
1100             ((info->cmap.green[fg_col] & 0xFC) << 3) |
1101             ((info->cmap.blue[fg_col] & 0xF8) >> 3);
1102 
1103         dev_dbg(fbi->dev, "fgcol %08lx, bgcol %08lx\n", fg, bg);
1104 
1105         smc501_writel(bg, base + SM501_OFF_HWC_COLOR_1_2);
1106         smc501_writel(fg, base + SM501_OFF_HWC_COLOR_3);
1107     }
1108 
1109     if (cursor->set & FB_CUR_SETSIZE ||
1110         cursor->set & (FB_CUR_SETIMAGE | FB_CUR_SETSHAPE)) {
1111         /* SM501 cursor is a two bpp 64x64 bitmap this routine
1112          * clears it to transparent then combines the cursor
1113          * shape plane with the colour plane to set the
1114          * cursor */
1115         int x, y;
1116         const unsigned char *pcol = cursor->image.data;
1117         const unsigned char *pmsk = cursor->mask;
1118         void __iomem   *dst = par->cursor.k_addr;
1119         unsigned char  dcol = 0;
1120         unsigned char  dmsk = 0;
1121         unsigned int   op;
1122 
1123         dev_dbg(fbi->dev, "%s: setting shape (%d,%d)\n",
1124             __func__, cursor->image.width, cursor->image.height);
1125 
1126         for (op = 0; op < (64*64*2)/8; op+=4)
1127             smc501_writel(0x0, dst + op);
1128 
1129         for (y = 0; y < cursor->image.height; y++) {
1130             for (x = 0; x < cursor->image.width; x++) {
1131                 if ((x % 8) == 0) {
1132                     dcol = *pcol++;
1133                     dmsk = *pmsk++;
1134                 } else {
1135                     dcol >>= 1;
1136                     dmsk >>= 1;
1137                 }
1138 
1139                 if (dmsk & 1) {
1140                     op = (dcol & 1) ? 1 : 3;
1141                     op <<= ((x % 4) * 2);
1142 
1143                     op |= readb(dst + (x / 4));
1144                     writeb(op, dst + (x / 4));
1145                 }
1146             }
1147             dst += (64*2)/8;
1148         }
1149     }
1150 
1151     sm501fb_sync_regs(fbi); /* ensure cursor data flushed */
1152     return 0;
1153 }
1154 
1155 /* sm501fb_crtsrc_show
1156  *
1157  * device attribute code to show where the crt output is sourced from
1158 */
1159 
1160 static ssize_t sm501fb_crtsrc_show(struct device *dev,
1161                    struct device_attribute *attr, char *buf)
1162 {
1163     struct sm501fb_info *info = dev_get_drvdata(dev);
1164     unsigned long ctrl;
1165 
1166     ctrl = smc501_readl(info->regs + SM501_DC_CRT_CONTROL);
1167     ctrl &= SM501_DC_CRT_CONTROL_SEL;
1168 
1169     return snprintf(buf, PAGE_SIZE, "%s\n", ctrl ? "crt" : "panel");
1170 }
1171 
1172 /* sm501fb_crtsrc_show
1173  *
1174  * device attribute code to set where the crt output is sourced from
1175 */
1176 
1177 static ssize_t sm501fb_crtsrc_store(struct device *dev,
1178                 struct device_attribute *attr,
1179                 const char *buf, size_t len)
1180 {
1181     struct sm501fb_info *info = dev_get_drvdata(dev);
1182     enum sm501_controller head;
1183     unsigned long ctrl;
1184 
1185     if (len < 1)
1186         return -EINVAL;
1187 
1188     if (strncasecmp(buf, "crt", 3) == 0)
1189         head = HEAD_CRT;
1190     else if (strncasecmp(buf, "panel", 5) == 0)
1191         head = HEAD_PANEL;
1192     else
1193         return -EINVAL;
1194 
1195     dev_info(dev, "setting crt source to head %d\n", head);
1196 
1197     ctrl = smc501_readl(info->regs + SM501_DC_CRT_CONTROL);
1198 
1199     if (head == HEAD_CRT) {
1200         ctrl |= SM501_DC_CRT_CONTROL_SEL;
1201         ctrl |= SM501_DC_CRT_CONTROL_ENABLE;
1202         ctrl |= SM501_DC_CRT_CONTROL_TE;
1203     } else {
1204         ctrl &= ~SM501_DC_CRT_CONTROL_SEL;
1205         ctrl &= ~SM501_DC_CRT_CONTROL_ENABLE;
1206         ctrl &= ~SM501_DC_CRT_CONTROL_TE;
1207     }
1208 
1209     smc501_writel(ctrl, info->regs + SM501_DC_CRT_CONTROL);
1210     sm501fb_sync_regs(info);
1211 
1212     return len;
1213 }
1214 
1215 /* Prepare the device_attr for registration with sysfs later */
1216 static DEVICE_ATTR(crt_src, 0664, sm501fb_crtsrc_show, sm501fb_crtsrc_store);
1217 
1218 /* sm501fb_show_regs
1219  *
1220  * show the primary sm501 registers
1221 */
1222 static int sm501fb_show_regs(struct sm501fb_info *info, char *ptr,
1223                  unsigned int start, unsigned int len)
1224 {
1225     void __iomem *mem = info->regs;
1226     char *buf = ptr;
1227     unsigned int reg;
1228 
1229     for (reg = start; reg < (len + start); reg += 4)
1230         ptr += sprintf(ptr, "%08x = %08x\n", reg,
1231                 smc501_readl(mem + reg));
1232 
1233     return ptr - buf;
1234 }
1235 
1236 /* sm501fb_debug_show_crt
1237  *
1238  * show the crt control and cursor registers
1239 */
1240 
1241 static ssize_t sm501fb_debug_show_crt(struct device *dev,
1242                   struct device_attribute *attr, char *buf)
1243 {
1244     struct sm501fb_info *info = dev_get_drvdata(dev);
1245     char *ptr = buf;
1246 
1247     ptr += sm501fb_show_regs(info, ptr, SM501_DC_CRT_CONTROL, 0x40);
1248     ptr += sm501fb_show_regs(info, ptr, SM501_DC_CRT_HWC_BASE, 0x10);
1249 
1250     return ptr - buf;
1251 }
1252 
1253 static DEVICE_ATTR(fbregs_crt, 0444, sm501fb_debug_show_crt, NULL);
1254 
1255 /* sm501fb_debug_show_pnl
1256  *
1257  * show the panel control and cursor registers
1258 */
1259 
1260 static ssize_t sm501fb_debug_show_pnl(struct device *dev,
1261                   struct device_attribute *attr, char *buf)
1262 {
1263     struct sm501fb_info *info = dev_get_drvdata(dev);
1264     char *ptr = buf;
1265 
1266     ptr += sm501fb_show_regs(info, ptr, 0x0, 0x40);
1267     ptr += sm501fb_show_regs(info, ptr, SM501_DC_PANEL_HWC_BASE, 0x10);
1268 
1269     return ptr - buf;
1270 }
1271 
1272 static DEVICE_ATTR(fbregs_pnl, 0444, sm501fb_debug_show_pnl, NULL);
1273 
1274 static struct attribute *sm501fb_attrs[] = {
1275     &dev_attr_crt_src.attr,
1276     &dev_attr_fbregs_pnl.attr,
1277     &dev_attr_fbregs_crt.attr,
1278     NULL,
1279 };
1280 ATTRIBUTE_GROUPS(sm501fb);
1281 
1282 /* acceleration operations */
1283 static int sm501fb_sync(struct fb_info *info)
1284 {
1285     int count = 1000000;
1286     struct sm501fb_par  *par = info->par;
1287     struct sm501fb_info *fbi = par->info;
1288 
1289     /* wait for the 2d engine to be ready */
1290     while ((count > 0) &&
1291            (smc501_readl(fbi->regs + SM501_SYSTEM_CONTROL) &
1292         SM501_SYSCTRL_2D_ENGINE_STATUS) != 0)
1293         count--;
1294 
1295     if (count <= 0) {
1296         dev_err(info->dev, "Timeout waiting for 2d engine sync\n");
1297         return 1;
1298     }
1299     return 0;
1300 }
1301 
1302 static void sm501fb_copyarea(struct fb_info *info, const struct fb_copyarea *area)
1303 {
1304     struct sm501fb_par  *par = info->par;
1305     struct sm501fb_info *fbi = par->info;
1306     int width = area->width;
1307     int height = area->height;
1308     int sx = area->sx;
1309     int sy = area->sy;
1310     int dx = area->dx;
1311     int dy = area->dy;
1312     unsigned long rtl = 0;
1313 
1314     /* source clip */
1315     if ((sx >= info->var.xres_virtual) ||
1316         (sy >= info->var.yres_virtual))
1317         /* source Area not within virtual screen, skipping */
1318         return;
1319     if ((sx + width) >= info->var.xres_virtual)
1320         width = info->var.xres_virtual - sx - 1;
1321     if ((sy + height) >= info->var.yres_virtual)
1322         height = info->var.yres_virtual - sy - 1;
1323 
1324     /* dest clip */
1325     if ((dx >= info->var.xres_virtual) ||
1326         (dy >= info->var.yres_virtual))
1327         /* Destination Area not within virtual screen, skipping */
1328         return;
1329     if ((dx + width) >= info->var.xres_virtual)
1330         width = info->var.xres_virtual - dx - 1;
1331     if ((dy + height) >= info->var.yres_virtual)
1332         height = info->var.yres_virtual - dy - 1;
1333 
1334     if ((sx < dx) || (sy < dy)) {
1335         rtl = 1 << 27;
1336         sx += width - 1;
1337         dx += width - 1;
1338         sy += height - 1;
1339         dy += height - 1;
1340     }
1341 
1342     if (sm501fb_sync(info))
1343         return;
1344 
1345     /* set the base addresses */
1346     smc501_writel(par->screen.sm_addr, fbi->regs2d + SM501_2D_SOURCE_BASE);
1347     smc501_writel(par->screen.sm_addr,
1348             fbi->regs2d + SM501_2D_DESTINATION_BASE);
1349 
1350     /* set the window width */
1351     smc501_writel((info->var.xres << 16) | info->var.xres,
1352            fbi->regs2d + SM501_2D_WINDOW_WIDTH);
1353 
1354     /* set window stride */
1355     smc501_writel((info->var.xres_virtual << 16) | info->var.xres_virtual,
1356            fbi->regs2d + SM501_2D_PITCH);
1357 
1358     /* set data format */
1359     switch (info->var.bits_per_pixel) {
1360     case 8:
1361         smc501_writel(0, fbi->regs2d + SM501_2D_STRETCH);
1362         break;
1363     case 16:
1364         smc501_writel(0x00100000, fbi->regs2d + SM501_2D_STRETCH);
1365         break;
1366     case 32:
1367         smc501_writel(0x00200000, fbi->regs2d + SM501_2D_STRETCH);
1368         break;
1369     }
1370 
1371     /* 2d compare mask */
1372     smc501_writel(0xffffffff, fbi->regs2d + SM501_2D_COLOR_COMPARE_MASK);
1373 
1374     /* 2d mask */
1375     smc501_writel(0xffffffff, fbi->regs2d + SM501_2D_MASK);
1376 
1377     /* source and destination x y */
1378     smc501_writel((sx << 16) | sy, fbi->regs2d + SM501_2D_SOURCE);
1379     smc501_writel((dx << 16) | dy, fbi->regs2d + SM501_2D_DESTINATION);
1380 
1381     /* w/h */
1382     smc501_writel((width << 16) | height, fbi->regs2d + SM501_2D_DIMENSION);
1383 
1384     /* do area move */
1385     smc501_writel(0x800000cc | rtl, fbi->regs2d + SM501_2D_CONTROL);
1386 }
1387 
1388 static void sm501fb_fillrect(struct fb_info *info, const struct fb_fillrect *rect)
1389 {
1390     struct sm501fb_par  *par = info->par;
1391     struct sm501fb_info *fbi = par->info;
1392     int width = rect->width, height = rect->height;
1393 
1394     if ((rect->dx >= info->var.xres_virtual) ||
1395         (rect->dy >= info->var.yres_virtual))
1396         /* Rectangle not within virtual screen, skipping */
1397         return;
1398     if ((rect->dx + width) >= info->var.xres_virtual)
1399         width = info->var.xres_virtual - rect->dx - 1;
1400     if ((rect->dy + height) >= info->var.yres_virtual)
1401         height = info->var.yres_virtual - rect->dy - 1;
1402 
1403     if (sm501fb_sync(info))
1404         return;
1405 
1406     /* set the base addresses */
1407     smc501_writel(par->screen.sm_addr, fbi->regs2d + SM501_2D_SOURCE_BASE);
1408     smc501_writel(par->screen.sm_addr,
1409             fbi->regs2d + SM501_2D_DESTINATION_BASE);
1410 
1411     /* set the window width */
1412     smc501_writel((info->var.xres << 16) | info->var.xres,
1413            fbi->regs2d + SM501_2D_WINDOW_WIDTH);
1414 
1415     /* set window stride */
1416     smc501_writel((info->var.xres_virtual << 16) | info->var.xres_virtual,
1417            fbi->regs2d + SM501_2D_PITCH);
1418 
1419     /* set data format */
1420     switch (info->var.bits_per_pixel) {
1421     case 8:
1422         smc501_writel(0, fbi->regs2d + SM501_2D_STRETCH);
1423         break;
1424     case 16:
1425         smc501_writel(0x00100000, fbi->regs2d + SM501_2D_STRETCH);
1426         break;
1427     case 32:
1428         smc501_writel(0x00200000, fbi->regs2d + SM501_2D_STRETCH);
1429         break;
1430     }
1431 
1432     /* 2d compare mask */
1433     smc501_writel(0xffffffff, fbi->regs2d + SM501_2D_COLOR_COMPARE_MASK);
1434 
1435     /* 2d mask */
1436     smc501_writel(0xffffffff, fbi->regs2d + SM501_2D_MASK);
1437 
1438     /* colour */
1439     smc501_writel(rect->color, fbi->regs2d + SM501_2D_FOREGROUND);
1440 
1441     /* x y */
1442     smc501_writel((rect->dx << 16) | rect->dy,
1443             fbi->regs2d + SM501_2D_DESTINATION);
1444 
1445     /* w/h */
1446     smc501_writel((width << 16) | height, fbi->regs2d + SM501_2D_DIMENSION);
1447 
1448     /* do rectangle fill */
1449     smc501_writel(0x800100cc, fbi->regs2d + SM501_2D_CONTROL);
1450 }
1451 
1452 
1453 static struct fb_ops sm501fb_ops_crt = {
1454     .owner      = THIS_MODULE,
1455     .fb_check_var   = sm501fb_check_var_crt,
1456     .fb_set_par = sm501fb_set_par_crt,
1457     .fb_blank   = sm501fb_blank_crt,
1458     .fb_setcolreg   = sm501fb_setcolreg,
1459     .fb_pan_display = sm501fb_pan_crt,
1460     .fb_cursor  = sm501fb_cursor,
1461     .fb_fillrect    = sm501fb_fillrect,
1462     .fb_copyarea    = sm501fb_copyarea,
1463     .fb_imageblit   = cfb_imageblit,
1464     .fb_sync    = sm501fb_sync,
1465 };
1466 
1467 static struct fb_ops sm501fb_ops_pnl = {
1468     .owner      = THIS_MODULE,
1469     .fb_check_var   = sm501fb_check_var_pnl,
1470     .fb_set_par = sm501fb_set_par_pnl,
1471     .fb_pan_display = sm501fb_pan_pnl,
1472     .fb_blank   = sm501fb_blank_pnl,
1473     .fb_setcolreg   = sm501fb_setcolreg,
1474     .fb_cursor  = sm501fb_cursor,
1475     .fb_fillrect    = sm501fb_fillrect,
1476     .fb_copyarea    = sm501fb_copyarea,
1477     .fb_imageblit   = cfb_imageblit,
1478     .fb_sync    = sm501fb_sync,
1479 };
1480 
1481 /* sm501_init_cursor
1482  *
1483  * initialise hw cursor parameters
1484 */
1485 
1486 static int sm501_init_cursor(struct fb_info *fbi, unsigned int reg_base)
1487 {
1488     struct sm501fb_par *par;
1489     struct sm501fb_info *info;
1490     int ret;
1491 
1492     if (fbi == NULL)
1493         return 0;
1494 
1495     par = fbi->par;
1496     info = par->info;
1497 
1498     par->cursor_regs = info->regs + reg_base;
1499 
1500     ret = sm501_alloc_mem(info, &par->cursor, SM501_MEMF_CURSOR, 1024,
1501                   fbi->fix.smem_len);
1502     if (ret < 0)
1503         return ret;
1504 
1505     /* initialise the colour registers */
1506 
1507     smc501_writel(par->cursor.sm_addr,
1508             par->cursor_regs + SM501_OFF_HWC_ADDR);
1509 
1510     smc501_writel(0x00, par->cursor_regs + SM501_OFF_HWC_LOC);
1511     smc501_writel(0x00, par->cursor_regs + SM501_OFF_HWC_COLOR_1_2);
1512     smc501_writel(0x00, par->cursor_regs + SM501_OFF_HWC_COLOR_3);
1513     sm501fb_sync_regs(info);
1514 
1515     return 0;
1516 }
1517 
1518 /* sm501fb_info_start
1519  *
1520  * fills the par structure claiming resources and remapping etc.
1521 */
1522 
1523 static int sm501fb_start(struct sm501fb_info *info,
1524              struct platform_device *pdev)
1525 {
1526     struct resource *res;
1527     struct device *dev = &pdev->dev;
1528     int k;
1529     int ret;
1530 
1531     info->irq = ret = platform_get_irq(pdev, 0);
1532     if (ret < 0) {
1533         /* we currently do not use the IRQ */
1534         dev_warn(dev, "no irq for device\n");
1535     }
1536 
1537     /* allocate, reserve and remap resources for display
1538      * controller registers */
1539     res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
1540     if (res == NULL) {
1541         dev_err(dev, "no resource definition for registers\n");
1542         ret = -ENOENT;
1543         goto err_release;
1544     }
1545 
1546     info->regs_res = request_mem_region(res->start,
1547                         resource_size(res),
1548                         pdev->name);
1549 
1550     if (info->regs_res == NULL) {
1551         dev_err(dev, "cannot claim registers\n");
1552         ret = -ENXIO;
1553         goto err_release;
1554     }
1555 
1556     info->regs = ioremap(res->start, resource_size(res));
1557     if (info->regs == NULL) {
1558         dev_err(dev, "cannot remap registers\n");
1559         ret = -ENXIO;
1560         goto err_regs_res;
1561     }
1562 
1563     /* allocate, reserve and remap resources for 2d
1564      * controller registers */
1565     res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
1566     if (res == NULL) {
1567         dev_err(dev, "no resource definition for 2d registers\n");
1568         ret = -ENOENT;
1569         goto err_regs_map;
1570     }
1571 
1572     info->regs2d_res = request_mem_region(res->start,
1573                           resource_size(res),
1574                           pdev->name);
1575 
1576     if (info->regs2d_res == NULL) {
1577         dev_err(dev, "cannot claim registers\n");
1578         ret = -ENXIO;
1579         goto err_regs_map;
1580     }
1581 
1582     info->regs2d = ioremap(res->start, resource_size(res));
1583     if (info->regs2d == NULL) {
1584         dev_err(dev, "cannot remap registers\n");
1585         ret = -ENXIO;
1586         goto err_regs2d_res;
1587     }
1588 
1589     /* allocate, reserve resources for framebuffer */
1590     res = platform_get_resource(pdev, IORESOURCE_MEM, 2);
1591     if (res == NULL) {
1592         dev_err(dev, "no memory resource defined\n");
1593         ret = -ENXIO;
1594         goto err_regs2d_map;
1595     }
1596 
1597     info->fbmem_res = request_mem_region(res->start,
1598                          resource_size(res),
1599                          pdev->name);
1600     if (info->fbmem_res == NULL) {
1601         dev_err(dev, "cannot claim framebuffer\n");
1602         ret = -ENXIO;
1603         goto err_regs2d_map;
1604     }
1605 
1606     info->fbmem = ioremap(res->start, resource_size(res));
1607     if (info->fbmem == NULL) {
1608         dev_err(dev, "cannot remap framebuffer\n");
1609         ret = -ENXIO;
1610         goto err_mem_res;
1611     }
1612 
1613     info->fbmem_len = resource_size(res);
1614 
1615     /* clear framebuffer memory - avoids garbage data on unused fb */
1616     memset_io(info->fbmem, 0, info->fbmem_len);
1617 
1618     /* clear palette ram - undefined at power on */
1619     for (k = 0; k < (256 * 3); k++)
1620         smc501_writel(0, info->regs + SM501_DC_PANEL_PALETTE + (k * 4));
1621 
1622     /* enable display controller */
1623     sm501_unit_power(dev->parent, SM501_GATE_DISPLAY, 1);
1624 
1625     /* enable 2d controller */
1626     sm501_unit_power(dev->parent, SM501_GATE_2D_ENGINE, 1);
1627 
1628     /* setup cursors */
1629     sm501_init_cursor(info->fb[HEAD_CRT], SM501_DC_CRT_HWC_ADDR);
1630     sm501_init_cursor(info->fb[HEAD_PANEL], SM501_DC_PANEL_HWC_ADDR);
1631 
1632     return 0; /* everything is setup */
1633 
1634  err_mem_res:
1635     release_mem_region(info->fbmem_res->start,
1636                resource_size(info->fbmem_res));
1637 
1638  err_regs2d_map:
1639     iounmap(info->regs2d);
1640 
1641  err_regs2d_res:
1642     release_mem_region(info->regs2d_res->start,
1643                resource_size(info->regs2d_res));
1644 
1645  err_regs_map:
1646     iounmap(info->regs);
1647 
1648  err_regs_res:
1649     release_mem_region(info->regs_res->start,
1650                resource_size(info->regs_res));
1651 
1652  err_release:
1653     return ret;
1654 }
1655 
1656 static void sm501fb_stop(struct sm501fb_info *info)
1657 {
1658     /* disable display controller */
1659     sm501_unit_power(info->dev->parent, SM501_GATE_DISPLAY, 0);
1660 
1661     iounmap(info->fbmem);
1662     release_mem_region(info->fbmem_res->start,
1663                resource_size(info->fbmem_res));
1664 
1665     iounmap(info->regs2d);
1666     release_mem_region(info->regs2d_res->start,
1667                resource_size(info->regs2d_res));
1668 
1669     iounmap(info->regs);
1670     release_mem_region(info->regs_res->start,
1671                resource_size(info->regs_res));
1672 }
1673 
1674 static int sm501fb_init_fb(struct fb_info *fb, enum sm501_controller head,
1675                const char *fbname)
1676 {
1677     struct sm501_platdata_fbsub *pd;
1678     struct sm501fb_par *par = fb->par;
1679     struct sm501fb_info *info = par->info;
1680     unsigned long ctrl;
1681     unsigned int enable;
1682     int ret;
1683 
1684     switch (head) {
1685     case HEAD_CRT:
1686         pd = info->pdata->fb_crt;
1687         ctrl = smc501_readl(info->regs + SM501_DC_CRT_CONTROL);
1688         enable = (ctrl & SM501_DC_CRT_CONTROL_ENABLE) ? 1 : 0;
1689 
1690         /* ensure we set the correct source register */
1691         if (info->pdata->fb_route != SM501_FB_CRT_PANEL) {
1692             ctrl |= SM501_DC_CRT_CONTROL_SEL;
1693             smc501_writel(ctrl, info->regs + SM501_DC_CRT_CONTROL);
1694         }
1695 
1696         break;
1697 
1698     case HEAD_PANEL:
1699         pd = info->pdata->fb_pnl;
1700         ctrl = smc501_readl(info->regs + SM501_DC_PANEL_CONTROL);
1701         enable = (ctrl & SM501_DC_PANEL_CONTROL_EN) ? 1 : 0;
1702         break;
1703 
1704     default:
1705         pd = NULL;      /* stop compiler warnings */
1706         ctrl = 0;
1707         enable = 0;
1708         BUG();
1709     }
1710 
1711     dev_info(info->dev, "fb %s %sabled at start\n",
1712          fbname, enable ? "en" : "dis");
1713 
1714     /* check to see if our routing allows this */
1715 
1716     if (head == HEAD_CRT && info->pdata->fb_route == SM501_FB_CRT_PANEL) {
1717         ctrl &= ~SM501_DC_CRT_CONTROL_SEL;
1718         smc501_writel(ctrl, info->regs + SM501_DC_CRT_CONTROL);
1719         enable = 0;
1720     }
1721 
1722     strscpy(fb->fix.id, fbname, sizeof(fb->fix.id));
1723 
1724     memcpy(&par->ops,
1725            (head == HEAD_CRT) ? &sm501fb_ops_crt : &sm501fb_ops_pnl,
1726            sizeof(struct fb_ops));
1727 
1728     /* update ops dependent on what we've been passed */
1729 
1730     if ((pd->flags & SM501FB_FLAG_USE_HWCURSOR) == 0)
1731         par->ops.fb_cursor = NULL;
1732 
1733     fb->fbops = &par->ops;
1734     fb->flags = FBINFO_FLAG_DEFAULT | FBINFO_READS_FAST |
1735         FBINFO_HWACCEL_COPYAREA | FBINFO_HWACCEL_FILLRECT |
1736         FBINFO_HWACCEL_XPAN | FBINFO_HWACCEL_YPAN;
1737 
1738 #if defined(CONFIG_OF)
1739 #ifdef __BIG_ENDIAN
1740     if (of_get_property(info->dev->parent->of_node, "little-endian", NULL))
1741         fb->flags |= FBINFO_FOREIGN_ENDIAN;
1742 #else
1743     if (of_get_property(info->dev->parent->of_node, "big-endian", NULL))
1744         fb->flags |= FBINFO_FOREIGN_ENDIAN;
1745 #endif
1746 #endif
1747     /* fixed data */
1748 
1749     fb->fix.type        = FB_TYPE_PACKED_PIXELS;
1750     fb->fix.type_aux    = 0;
1751     fb->fix.xpanstep    = 1;
1752     fb->fix.ypanstep    = 1;
1753     fb->fix.ywrapstep   = 0;
1754     fb->fix.accel       = FB_ACCEL_NONE;
1755 
1756     /* screenmode */
1757 
1758     fb->var.nonstd      = 0;
1759     fb->var.activate    = FB_ACTIVATE_NOW;
1760     fb->var.accel_flags = 0;
1761     fb->var.vmode       = FB_VMODE_NONINTERLACED;
1762     fb->var.bits_per_pixel  = 16;
1763 
1764     if (info->edid_data) {
1765             /* Now build modedb from EDID */
1766             fb_edid_to_monspecs(info->edid_data, &fb->monspecs);
1767             fb_videomode_to_modelist(fb->monspecs.modedb,
1768                          fb->monspecs.modedb_len,
1769                          &fb->modelist);
1770     }
1771 
1772     if (enable && (pd->flags & SM501FB_FLAG_USE_INIT_MODE) && 0) {
1773         /* TODO read the mode from the current display */
1774     } else {
1775         if (pd->def_mode) {
1776             dev_info(info->dev, "using supplied mode\n");
1777             fb_videomode_to_var(&fb->var, pd->def_mode);
1778 
1779             fb->var.bits_per_pixel = pd->def_bpp ? pd->def_bpp : 8;
1780             fb->var.xres_virtual = fb->var.xres;
1781             fb->var.yres_virtual = fb->var.yres;
1782         } else {
1783             if (info->edid_data) {
1784                 ret = fb_find_mode(&fb->var, fb, fb_mode,
1785                     fb->monspecs.modedb,
1786                     fb->monspecs.modedb_len,
1787                     &sm501_default_mode, default_bpp);
1788                 /* edid_data is no longer needed, free it */
1789                 kfree(info->edid_data);
1790             } else {
1791                 ret = fb_find_mode(&fb->var, fb,
1792                        NULL, NULL, 0, NULL, 8);
1793             }
1794 
1795             switch (ret) {
1796             case 1:
1797                 dev_info(info->dev, "using mode specified in "
1798                         "@mode\n");
1799                 break;
1800             case 2:
1801                 dev_info(info->dev, "using mode specified in "
1802                     "@mode with ignored refresh rate\n");
1803                 break;
1804             case 3:
1805                 dev_info(info->dev, "using mode default "
1806                     "mode\n");
1807                 break;
1808             case 4:
1809                 dev_info(info->dev, "using mode from list\n");
1810                 break;
1811             default:
1812                 dev_info(info->dev, "ret = %d\n", ret);
1813                 dev_info(info->dev, "failed to find mode\n");
1814                 return -EINVAL;
1815             }
1816         }
1817     }
1818 
1819     /* initialise and set the palette */
1820     if (fb_alloc_cmap(&fb->cmap, NR_PALETTE, 0)) {
1821         dev_err(info->dev, "failed to allocate cmap memory\n");
1822         return -ENOMEM;
1823     }
1824     fb_set_cmap(&fb->cmap, fb);
1825 
1826     ret = (fb->fbops->fb_check_var)(&fb->var, fb);
1827     if (ret)
1828         dev_err(info->dev, "check_var() failed on initial setup?\n");
1829 
1830     return 0;
1831 }
1832 
1833 /* default platform data if none is supplied (ie, PCI device) */
1834 
1835 static struct sm501_platdata_fbsub sm501fb_pdata_crt = {
1836     .flags      = (SM501FB_FLAG_USE_INIT_MODE |
1837                SM501FB_FLAG_USE_HWCURSOR |
1838                SM501FB_FLAG_USE_HWACCEL |
1839                SM501FB_FLAG_DISABLE_AT_EXIT),
1840 
1841 };
1842 
1843 static struct sm501_platdata_fbsub sm501fb_pdata_pnl = {
1844     .flags      = (SM501FB_FLAG_USE_INIT_MODE |
1845                SM501FB_FLAG_USE_HWCURSOR |
1846                SM501FB_FLAG_USE_HWACCEL |
1847                SM501FB_FLAG_DISABLE_AT_EXIT),
1848 };
1849 
1850 static struct sm501_platdata_fb sm501fb_def_pdata = {
1851     .fb_route       = SM501_FB_OWN,
1852     .fb_crt         = &sm501fb_pdata_crt,
1853     .fb_pnl         = &sm501fb_pdata_pnl,
1854 };
1855 
1856 static char driver_name_crt[] = "sm501fb-crt";
1857 static char driver_name_pnl[] = "sm501fb-panel";
1858 
1859 static int sm501fb_probe_one(struct sm501fb_info *info,
1860                  enum sm501_controller head)
1861 {
1862     unsigned char *name = (head == HEAD_CRT) ? "crt" : "panel";
1863     struct sm501_platdata_fbsub *pd;
1864     struct sm501fb_par *par;
1865     struct fb_info *fbi;
1866 
1867     pd = (head == HEAD_CRT) ? info->pdata->fb_crt : info->pdata->fb_pnl;
1868 
1869     /* Do not initialise if we've not been given any platform data */
1870     if (pd == NULL) {
1871         dev_info(info->dev, "no data for fb %s (disabled)\n", name);
1872         return 0;
1873     }
1874 
1875     fbi = framebuffer_alloc(sizeof(struct sm501fb_par), info->dev);
1876     if (!fbi)
1877         return -ENOMEM;
1878 
1879     par = fbi->par;
1880     par->info = info;
1881     par->head = head;
1882     fbi->pseudo_palette = &par->pseudo_palette;
1883 
1884     info->fb[head] = fbi;
1885 
1886     return 0;
1887 }
1888 
1889 /* Free up anything allocated by sm501fb_init_fb */
1890 
1891 static void sm501_free_init_fb(struct sm501fb_info *info,
1892                 enum sm501_controller head)
1893 {
1894     struct fb_info *fbi = info->fb[head];
1895 
1896     if (!fbi)
1897         return;
1898 
1899     fb_dealloc_cmap(&fbi->cmap);
1900 }
1901 
1902 static int sm501fb_start_one(struct sm501fb_info *info,
1903                  enum sm501_controller head, const char *drvname)
1904 {
1905     struct fb_info *fbi = info->fb[head];
1906     int ret;
1907 
1908     if (!fbi)
1909         return 0;
1910 
1911     mutex_init(&info->fb[head]->mm_lock);
1912 
1913     ret = sm501fb_init_fb(info->fb[head], head, drvname);
1914     if (ret) {
1915         dev_err(info->dev, "cannot initialise fb %s\n", drvname);
1916         return ret;
1917     }
1918 
1919     ret = register_framebuffer(info->fb[head]);
1920     if (ret) {
1921         dev_err(info->dev, "failed to register fb %s\n", drvname);
1922         sm501_free_init_fb(info, head);
1923         return ret;
1924     }
1925 
1926     dev_info(info->dev, "fb%d: %s frame buffer\n", fbi->node, fbi->fix.id);
1927 
1928     return 0;
1929 }
1930 
1931 static int sm501fb_probe(struct platform_device *pdev)
1932 {
1933     struct sm501fb_info *info;
1934     struct device *dev = &pdev->dev;
1935     int ret;
1936 
1937     /* allocate our framebuffers */
1938     info = kzalloc(sizeof(*info), GFP_KERNEL);
1939     if (!info) {
1940         dev_err(dev, "failed to allocate state\n");
1941         return -ENOMEM;
1942     }
1943 
1944     info->dev = dev = &pdev->dev;
1945     platform_set_drvdata(pdev, info);
1946 
1947     if (dev->parent->platform_data) {
1948         struct sm501_platdata *pd = dev->parent->platform_data;
1949         info->pdata = pd->fb;
1950     }
1951 
1952     if (info->pdata == NULL) {
1953         int found = 0;
1954 #if defined(CONFIG_OF)
1955         struct device_node *np = pdev->dev.parent->of_node;
1956         const u8 *prop;
1957         const char *cp;
1958         int len;
1959 
1960         info->pdata = &sm501fb_def_pdata;
1961         if (np) {
1962             /* Get EDID */
1963             cp = of_get_property(np, "mode", &len);
1964             if (cp)
1965                 strcpy(fb_mode, cp);
1966             prop = of_get_property(np, "edid", &len);
1967             if (prop && len == EDID_LENGTH) {
1968                 info->edid_data = kmemdup(prop, EDID_LENGTH,
1969                               GFP_KERNEL);
1970                 if (info->edid_data)
1971                     found = 1;
1972             }
1973         }
1974 #endif
1975         if (!found) {
1976             dev_info(dev, "using default configuration data\n");
1977             info->pdata = &sm501fb_def_pdata;
1978         }
1979     }
1980 
1981     /* probe for the presence of each panel */
1982 
1983     ret = sm501fb_probe_one(info, HEAD_CRT);
1984     if (ret < 0) {
1985         dev_err(dev, "failed to probe CRT\n");
1986         goto err_alloc;
1987     }
1988 
1989     ret = sm501fb_probe_one(info, HEAD_PANEL);
1990     if (ret < 0) {
1991         dev_err(dev, "failed to probe PANEL\n");
1992         goto err_probed_crt;
1993     }
1994 
1995     if (info->fb[HEAD_PANEL] == NULL &&
1996         info->fb[HEAD_CRT] == NULL) {
1997         dev_err(dev, "no framebuffers found\n");
1998         ret = -ENODEV;
1999         goto err_alloc;
2000     }
2001 
2002     /* get the resources for both of the framebuffers */
2003 
2004     ret = sm501fb_start(info, pdev);
2005     if (ret) {
2006         dev_err(dev, "cannot initialise SM501\n");
2007         goto err_probed_panel;
2008     }
2009 
2010     ret = sm501fb_start_one(info, HEAD_CRT, driver_name_crt);
2011     if (ret) {
2012         dev_err(dev, "failed to start CRT\n");
2013         goto err_started;
2014     }
2015 
2016     ret = sm501fb_start_one(info, HEAD_PANEL, driver_name_pnl);
2017     if (ret) {
2018         dev_err(dev, "failed to start Panel\n");
2019         goto err_started_crt;
2020     }
2021 
2022     /* we registered, return ok */
2023     return 0;
2024 
2025 err_started_crt:
2026     unregister_framebuffer(info->fb[HEAD_CRT]);
2027     sm501_free_init_fb(info, HEAD_CRT);
2028 
2029 err_started:
2030     sm501fb_stop(info);
2031 
2032 err_probed_panel:
2033     framebuffer_release(info->fb[HEAD_PANEL]);
2034 
2035 err_probed_crt:
2036     framebuffer_release(info->fb[HEAD_CRT]);
2037 
2038 err_alloc:
2039     kfree(info);
2040 
2041     return ret;
2042 }
2043 
2044 
2045 /*
2046  *  Cleanup
2047  */
2048 static int sm501fb_remove(struct platform_device *pdev)
2049 {
2050     struct sm501fb_info *info = platform_get_drvdata(pdev);
2051     struct fb_info     *fbinfo_crt = info->fb[0];
2052     struct fb_info     *fbinfo_pnl = info->fb[1];
2053 
2054     sm501_free_init_fb(info, HEAD_CRT);
2055     sm501_free_init_fb(info, HEAD_PANEL);
2056 
2057     if (fbinfo_crt)
2058         unregister_framebuffer(fbinfo_crt);
2059     if (fbinfo_pnl)
2060         unregister_framebuffer(fbinfo_pnl);
2061 
2062     sm501fb_stop(info);
2063     kfree(info);
2064 
2065     framebuffer_release(fbinfo_pnl);
2066     framebuffer_release(fbinfo_crt);
2067 
2068     return 0;
2069 }
2070 
2071 #ifdef CONFIG_PM
2072 
2073 static int sm501fb_suspend_fb(struct sm501fb_info *info,
2074                   enum sm501_controller head)
2075 {
2076     struct fb_info *fbi = info->fb[head];
2077     struct sm501fb_par *par;
2078 
2079     if (!fbi)
2080         return 0;
2081 
2082     par = fbi->par;
2083     if (par->screen.size == 0)
2084         return 0;
2085 
2086     /* blank the relevant interface to ensure unit power minimised */
2087     (par->ops.fb_blank)(FB_BLANK_POWERDOWN, fbi);
2088 
2089     /* tell console/fb driver we are suspending */
2090 
2091     console_lock();
2092     fb_set_suspend(fbi, 1);
2093     console_unlock();
2094 
2095     /* backup copies in case chip is powered down over suspend */
2096 
2097     par->store_fb = vmalloc(par->screen.size);
2098     if (par->store_fb == NULL) {
2099         dev_err(info->dev, "no memory to store screen\n");
2100         return -ENOMEM;
2101     }
2102 
2103     par->store_cursor = vmalloc(par->cursor.size);
2104     if (par->store_cursor == NULL) {
2105         dev_err(info->dev, "no memory to store cursor\n");
2106         goto err_nocursor;
2107     }
2108 
2109     dev_dbg(info->dev, "suspending screen to %p\n", par->store_fb);
2110     dev_dbg(info->dev, "suspending cursor to %p\n", par->store_cursor);
2111 
2112     memcpy_fromio(par->store_fb, par->screen.k_addr, par->screen.size);
2113     memcpy_fromio(par->store_cursor, par->cursor.k_addr, par->cursor.size);
2114 
2115     return 0;
2116 
2117  err_nocursor:
2118     vfree(par->store_fb);
2119     par->store_fb = NULL;
2120 
2121     return -ENOMEM;
2122 }
2123 
2124 static void sm501fb_resume_fb(struct sm501fb_info *info,
2125                   enum sm501_controller head)
2126 {
2127     struct fb_info *fbi = info->fb[head];
2128     struct sm501fb_par *par;
2129 
2130     if (!fbi)
2131         return;
2132 
2133     par = fbi->par;
2134     if (par->screen.size == 0)
2135         return;
2136 
2137     /* re-activate the configuration */
2138 
2139     (par->ops.fb_set_par)(fbi);
2140 
2141     /* restore the data */
2142 
2143     dev_dbg(info->dev, "restoring screen from %p\n", par->store_fb);
2144     dev_dbg(info->dev, "restoring cursor from %p\n", par->store_cursor);
2145 
2146     if (par->store_fb)
2147         memcpy_toio(par->screen.k_addr, par->store_fb,
2148                 par->screen.size);
2149 
2150     if (par->store_cursor)
2151         memcpy_toio(par->cursor.k_addr, par->store_cursor,
2152                 par->cursor.size);
2153 
2154     console_lock();
2155     fb_set_suspend(fbi, 0);
2156     console_unlock();
2157 
2158     vfree(par->store_fb);
2159     vfree(par->store_cursor);
2160 }
2161 
2162 
2163 /* suspend and resume support */
2164 
2165 static int sm501fb_suspend(struct platform_device *pdev, pm_message_t state)
2166 {
2167     struct sm501fb_info *info = platform_get_drvdata(pdev);
2168 
2169     /* store crt control to resume with */
2170     info->pm_crt_ctrl = smc501_readl(info->regs + SM501_DC_CRT_CONTROL);
2171 
2172     sm501fb_suspend_fb(info, HEAD_CRT);
2173     sm501fb_suspend_fb(info, HEAD_PANEL);
2174 
2175     /* turn off the clocks, in case the device is not powered down */
2176     sm501_unit_power(info->dev->parent, SM501_GATE_DISPLAY, 0);
2177 
2178     return 0;
2179 }
2180 
2181 #define SM501_CRT_CTRL_SAVE (SM501_DC_CRT_CONTROL_TVP |        \
2182                  SM501_DC_CRT_CONTROL_SEL)
2183 
2184 
2185 static int sm501fb_resume(struct platform_device *pdev)
2186 {
2187     struct sm501fb_info *info = platform_get_drvdata(pdev);
2188     unsigned long crt_ctrl;
2189 
2190     sm501_unit_power(info->dev->parent, SM501_GATE_DISPLAY, 1);
2191 
2192     /* restore the items we want to be saved for crt control */
2193 
2194     crt_ctrl = smc501_readl(info->regs + SM501_DC_CRT_CONTROL);
2195     crt_ctrl &= ~SM501_CRT_CTRL_SAVE;
2196     crt_ctrl |= info->pm_crt_ctrl & SM501_CRT_CTRL_SAVE;
2197     smc501_writel(crt_ctrl, info->regs + SM501_DC_CRT_CONTROL);
2198 
2199     sm501fb_resume_fb(info, HEAD_CRT);
2200     sm501fb_resume_fb(info, HEAD_PANEL);
2201 
2202     return 0;
2203 }
2204 
2205 #else
2206 #define sm501fb_suspend NULL
2207 #define sm501fb_resume  NULL
2208 #endif
2209 
2210 static struct platform_driver sm501fb_driver = {
2211     .probe      = sm501fb_probe,
2212     .remove     = sm501fb_remove,
2213     .suspend    = sm501fb_suspend,
2214     .resume     = sm501fb_resume,
2215     .driver     = {
2216         .name   = "sm501-fb",
2217         .dev_groups = sm501fb_groups,
2218     },
2219 };
2220 
2221 module_platform_driver(sm501fb_driver);
2222 
2223 module_param_named(mode, fb_mode, charp, 0);
2224 MODULE_PARM_DESC(mode,
2225     "Specify resolution as \"<xres>x<yres>[-<bpp>][@<refresh>]\" ");
2226 module_param_named(bpp, default_bpp, ulong, 0);
2227 MODULE_PARM_DESC(bpp, "Specify bit-per-pixel if not specified mode");
2228 MODULE_AUTHOR("Ben Dooks, Vincent Sanders");
2229 MODULE_DESCRIPTION("SM501 Framebuffer driver");
2230 MODULE_LICENSE("GPL v2");
[ Source navigation ] [ Diff markup ] [ Identifier search ] [ general search ]

This page was automatically generated by the 2.1.0 LXR engine.

The LXR team
Valid CSS 2.1! Valid HTML 4.01!