sm501fb.c

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