1 /*
2  *  Driver for AT91/AT32 LCD Controller
3  *
4  *  Copyright (C) 2007 Atmel Corporation
5  *
6  * This file is subject to the terms and conditions of the GNU General Public
7  * License.  See the file COPYING in the main directory of this archive for
8  * more details.
9  */
10 
11 #include <linux/kernel.h>
12 #include <linux/platform_device.h>
13 #include <linux/dma-mapping.h>
14 #include <linux/interrupt.h>
15 #include <linux/clk.h>
16 #include <linux/fb.h>
17 #include <linux/init.h>
18 #include <linux/delay.h>
19 #include <linux/backlight.h>
20 #include <linux/gfp.h>
21 #include <linux/gpio/consumer.h>
22 #include <linux/module.h>
23 #include <linux/of.h>
24 #include <linux/of_device.h>
25 #include <video/of_videomode.h>
26 #include <video/of_display_timing.h>
27 #include <linux/regulator/consumer.h>
28 #include <video/videomode.h>
29 
30 #include <video/atmel_lcdc.h>
31 
32 struct atmel_lcdfb_config {
33 	bool have_alt_pixclock;
34 	bool have_hozval;
35 	bool have_intensity_bit;
36 };
37 
38  /* LCD Controller info data structure, stored in device platform_data */
39 struct atmel_lcdfb_info {
40 	spinlock_t		lock;
41 	struct fb_info		*info;
42 	void __iomem		*mmio;
43 	int			irq_base;
44 	struct work_struct	task;
45 
46 	unsigned int		smem_len;
47 	struct platform_device	*pdev;
48 	struct clk		*bus_clk;
49 	struct clk		*lcdc_clk;
50 
51 	struct backlight_device	*backlight;
52 	u8			bl_power;
53 	u8			saved_lcdcon;
54 
55 	u32			pseudo_palette[16];
56 	bool			have_intensity_bit;
57 
58 	struct atmel_lcdfb_pdata pdata;
59 
60 	struct atmel_lcdfb_config *config;
61 	struct regulator	*reg_lcd;
62 };
63 
64 struct atmel_lcdfb_power_ctrl_gpio {
65 	struct gpio_desc *gpiod;
66 
67 	struct list_head list;
68 };
69 
70 #define lcdc_readl(sinfo, reg)		__raw_readl((sinfo)->mmio+(reg))
71 #define lcdc_writel(sinfo, reg, val)	__raw_writel((val), (sinfo)->mmio+(reg))
72 
73 /* configurable parameters */
74 #define ATMEL_LCDC_CVAL_DEFAULT		0xc8
75 #define ATMEL_LCDC_DMA_BURST_LEN	8	/* words */
76 #define ATMEL_LCDC_FIFO_SIZE		512	/* words */
77 
78 static struct atmel_lcdfb_config at91sam9261_config = {
79 	.have_hozval		= true,
80 	.have_intensity_bit	= true,
81 };
82 
83 static struct atmel_lcdfb_config at91sam9263_config = {
84 	.have_intensity_bit	= true,
85 };
86 
87 static struct atmel_lcdfb_config at91sam9g10_config = {
88 	.have_hozval		= true,
89 };
90 
91 static struct atmel_lcdfb_config at91sam9g45_config = {
92 	.have_alt_pixclock	= true,
93 };
94 
95 static struct atmel_lcdfb_config at91sam9g45es_config = {
96 };
97 
98 static struct atmel_lcdfb_config at91sam9rl_config = {
99 	.have_intensity_bit	= true,
100 };
101 
102 static struct atmel_lcdfb_config at32ap_config = {
103 	.have_hozval		= true,
104 };
105 
106 static const struct platform_device_id atmel_lcdfb_devtypes[] = {
107 	{
108 		.name = "at91sam9261-lcdfb",
109 		.driver_data = (unsigned long)&at91sam9261_config,
110 	}, {
111 		.name = "at91sam9263-lcdfb",
112 		.driver_data = (unsigned long)&at91sam9263_config,
113 	}, {
114 		.name = "at91sam9g10-lcdfb",
115 		.driver_data = (unsigned long)&at91sam9g10_config,
116 	}, {
117 		.name = "at91sam9g45-lcdfb",
118 		.driver_data = (unsigned long)&at91sam9g45_config,
119 	}, {
120 		.name = "at91sam9g45es-lcdfb",
121 		.driver_data = (unsigned long)&at91sam9g45es_config,
122 	}, {
123 		.name = "at91sam9rl-lcdfb",
124 		.driver_data = (unsigned long)&at91sam9rl_config,
125 	}, {
126 		.name = "at32ap-lcdfb",
127 		.driver_data = (unsigned long)&at32ap_config,
128 	}, {
129 		/* terminator */
130 	}
131 };
132 MODULE_DEVICE_TABLE(platform, atmel_lcdfb_devtypes);
133 
134 static struct atmel_lcdfb_config *
135 atmel_lcdfb_get_config(struct platform_device *pdev)
136 {
137 	unsigned long data;
138 
139 	data = platform_get_device_id(pdev)->driver_data;
140 
141 	return (struct atmel_lcdfb_config *)data;
142 }
143 
144 #if defined(CONFIG_ARCH_AT91)
145 #define	ATMEL_LCDFB_FBINFO_DEFAULT	(FBINFO_DEFAULT \
146 					 | FBINFO_PARTIAL_PAN_OK \
147 					 | FBINFO_HWACCEL_YPAN)
148 
149 static inline void atmel_lcdfb_update_dma2d(struct atmel_lcdfb_info *sinfo,
150 					struct fb_var_screeninfo *var,
151 					struct fb_info *info)
152 {
153 
154 }
155 #elif defined(CONFIG_AVR32)
156 #define	ATMEL_LCDFB_FBINFO_DEFAULT	(FBINFO_DEFAULT \
157 					| FBINFO_PARTIAL_PAN_OK \
158 					| FBINFO_HWACCEL_XPAN \
159 					| FBINFO_HWACCEL_YPAN)
160 
161 static void atmel_lcdfb_update_dma2d(struct atmel_lcdfb_info *sinfo,
162 				     struct fb_var_screeninfo *var,
163 				     struct fb_info *info)
164 {
165 	u32 dma2dcfg;
166 	u32 pixeloff;
167 
168 	pixeloff = (var->xoffset * info->var.bits_per_pixel) & 0x1f;
169 
170 	dma2dcfg = (info->var.xres_virtual - info->var.xres)
171 		 * info->var.bits_per_pixel / 8;
172 	dma2dcfg |= pixeloff << ATMEL_LCDC_PIXELOFF_OFFSET;
173 	lcdc_writel(sinfo, ATMEL_LCDC_DMA2DCFG, dma2dcfg);
174 
175 	/* Update configuration */
176 	lcdc_writel(sinfo, ATMEL_LCDC_DMACON,
177 		    lcdc_readl(sinfo, ATMEL_LCDC_DMACON)
178 		    | ATMEL_LCDC_DMAUPDT);
179 }
180 #endif
181 
182 static u32 contrast_ctr = ATMEL_LCDC_PS_DIV8
183 		| ATMEL_LCDC_POL_POSITIVE
184 		| ATMEL_LCDC_ENA_PWMENABLE;
185 
186 #ifdef CONFIG_BACKLIGHT_ATMEL_LCDC
187 
188 /* some bl->props field just changed */
189 static int atmel_bl_update_status(struct backlight_device *bl)
190 {
191 	struct atmel_lcdfb_info *sinfo = bl_get_data(bl);
192 	int			power = sinfo->bl_power;
193 	int			brightness = bl->props.brightness;
194 
195 	/* REVISIT there may be a meaningful difference between
196 	 * fb_blank and power ... there seem to be some cases
197 	 * this doesn't handle correctly.
198 	 */
199 	if (bl->props.fb_blank != sinfo->bl_power)
200 		power = bl->props.fb_blank;
201 	else if (bl->props.power != sinfo->bl_power)
202 		power = bl->props.power;
203 
204 	if (brightness < 0 && power == FB_BLANK_UNBLANK)
205 		brightness = lcdc_readl(sinfo, ATMEL_LCDC_CONTRAST_VAL);
206 	else if (power != FB_BLANK_UNBLANK)
207 		brightness = 0;
208 
209 	lcdc_writel(sinfo, ATMEL_LCDC_CONTRAST_VAL, brightness);
210 	if (contrast_ctr & ATMEL_LCDC_POL_POSITIVE)
211 		lcdc_writel(sinfo, ATMEL_LCDC_CONTRAST_CTR,
212 			brightness ? contrast_ctr : 0);
213 	else
214 		lcdc_writel(sinfo, ATMEL_LCDC_CONTRAST_CTR, contrast_ctr);
215 
216 	bl->props.fb_blank = bl->props.power = sinfo->bl_power = power;
217 
218 	return 0;
219 }
220 
221 static int atmel_bl_get_brightness(struct backlight_device *bl)
222 {
223 	struct atmel_lcdfb_info *sinfo = bl_get_data(bl);
224 
225 	return lcdc_readl(sinfo, ATMEL_LCDC_CONTRAST_VAL);
226 }
227 
228 static const struct backlight_ops atmel_lcdc_bl_ops = {
229 	.update_status = atmel_bl_update_status,
230 	.get_brightness = atmel_bl_get_brightness,
231 };
232 
233 static void init_backlight(struct atmel_lcdfb_info *sinfo)
234 {
235 	struct backlight_properties props;
236 	struct backlight_device	*bl;
237 
238 	sinfo->bl_power = FB_BLANK_UNBLANK;
239 
240 	if (sinfo->backlight)
241 		return;
242 
243 	memset(&props, 0, sizeof(struct backlight_properties));
244 	props.type = BACKLIGHT_RAW;
245 	props.max_brightness = 0xff;
246 	bl = backlight_device_register("backlight", &sinfo->pdev->dev, sinfo,
247 				       &atmel_lcdc_bl_ops, &props);
248 	if (IS_ERR(bl)) {
249 		dev_err(&sinfo->pdev->dev, "error %ld on backlight register\n",
250 				PTR_ERR(bl));
251 		return;
252 	}
253 	sinfo->backlight = bl;
254 
255 	bl->props.power = FB_BLANK_UNBLANK;
256 	bl->props.fb_blank = FB_BLANK_UNBLANK;
257 	bl->props.brightness = atmel_bl_get_brightness(bl);
258 }
259 
260 static void exit_backlight(struct atmel_lcdfb_info *sinfo)
261 {
262 	if (!sinfo->backlight)
263 		return;
264 
265 	if (sinfo->backlight->ops) {
266 		sinfo->backlight->props.power = FB_BLANK_POWERDOWN;
267 		sinfo->backlight->ops->update_status(sinfo->backlight);
268 	}
269 	backlight_device_unregister(sinfo->backlight);
270 }
271 
272 #else
273 
274 static void init_backlight(struct atmel_lcdfb_info *sinfo)
275 {
276 	dev_warn(&sinfo->pdev->dev, "backlight control is not available\n");
277 }
278 
279 static void exit_backlight(struct atmel_lcdfb_info *sinfo)
280 {
281 }
282 
283 #endif
284 
285 static void init_contrast(struct atmel_lcdfb_info *sinfo)
286 {
287 	struct atmel_lcdfb_pdata *pdata = &sinfo->pdata;
288 
289 	/* contrast pwm can be 'inverted' */
290 	if (pdata->lcdcon_pol_negative)
291 		contrast_ctr &= ~(ATMEL_LCDC_POL_POSITIVE);
292 
293 	/* have some default contrast/backlight settings */
294 	lcdc_writel(sinfo, ATMEL_LCDC_CONTRAST_CTR, contrast_ctr);
295 	lcdc_writel(sinfo, ATMEL_LCDC_CONTRAST_VAL, ATMEL_LCDC_CVAL_DEFAULT);
296 
297 	if (pdata->lcdcon_is_backlight)
298 		init_backlight(sinfo);
299 }
300 
301 static inline void atmel_lcdfb_power_control(struct atmel_lcdfb_info *sinfo, int on)
302 {
303 	int ret;
304 	struct atmel_lcdfb_pdata *pdata = &sinfo->pdata;
305 
306 	if (pdata->atmel_lcdfb_power_control)
307 		pdata->atmel_lcdfb_power_control(pdata, on);
308 	else if (sinfo->reg_lcd) {
309 		if (on) {
310 			ret = regulator_enable(sinfo->reg_lcd);
311 			if (ret)
312 				dev_err(&sinfo->pdev->dev,
313 					"lcd regulator enable failed:	%d\n", ret);
314 		} else {
315 			ret = regulator_disable(sinfo->reg_lcd);
316 			if (ret)
317 				dev_err(&sinfo->pdev->dev,
318 					"lcd regulator disable failed: %d\n", ret);
319 		}
320 	}
321 }
322 
323 static const struct fb_fix_screeninfo atmel_lcdfb_fix __initconst = {
324 	.type		= FB_TYPE_PACKED_PIXELS,
325 	.visual		= FB_VISUAL_TRUECOLOR,
326 	.xpanstep	= 0,
327 	.ypanstep	= 1,
328 	.ywrapstep	= 0,
329 	.accel		= FB_ACCEL_NONE,
330 };
331 
332 static unsigned long compute_hozval(struct atmel_lcdfb_info *sinfo,
333 							unsigned long xres)
334 {
335 	unsigned long lcdcon2;
336 	unsigned long value;
337 
338 	if (!sinfo->config->have_hozval)
339 		return xres;
340 
341 	lcdcon2 = lcdc_readl(sinfo, ATMEL_LCDC_LCDCON2);
342 	value = xres;
343 	if ((lcdcon2 & ATMEL_LCDC_DISTYPE) != ATMEL_LCDC_DISTYPE_TFT) {
344 		/* STN display */
345 		if ((lcdcon2 & ATMEL_LCDC_DISTYPE) == ATMEL_LCDC_DISTYPE_STNCOLOR) {
346 			value *= 3;
347 		}
348 		if ( (lcdcon2 & ATMEL_LCDC_IFWIDTH) == ATMEL_LCDC_IFWIDTH_4
349 		   || ( (lcdcon2 & ATMEL_LCDC_IFWIDTH) == ATMEL_LCDC_IFWIDTH_8
350 		      && (lcdcon2 & ATMEL_LCDC_SCANMOD) == ATMEL_LCDC_SCANMOD_DUAL ))
351 			value = DIV_ROUND_UP(value, 4);
352 		else
353 			value = DIV_ROUND_UP(value, 8);
354 	}
355 
356 	return value;
357 }
358 
359 static void atmel_lcdfb_stop_nowait(struct atmel_lcdfb_info *sinfo)
360 {
361 	struct atmel_lcdfb_pdata *pdata = &sinfo->pdata;
362 
363 	/* Turn off the LCD controller and the DMA controller */
364 	lcdc_writel(sinfo, ATMEL_LCDC_PWRCON,
365 			pdata->guard_time << ATMEL_LCDC_GUARDT_OFFSET);
366 
367 	/* Wait for the LCDC core to become idle */
368 	while (lcdc_readl(sinfo, ATMEL_LCDC_PWRCON) & ATMEL_LCDC_BUSY)
369 		msleep(10);
370 
371 	lcdc_writel(sinfo, ATMEL_LCDC_DMACON, 0);
372 }
373 
374 static void atmel_lcdfb_stop(struct atmel_lcdfb_info *sinfo)
375 {
376 	atmel_lcdfb_stop_nowait(sinfo);
377 
378 	/* Wait for DMA engine to become idle... */
379 	while (lcdc_readl(sinfo, ATMEL_LCDC_DMACON) & ATMEL_LCDC_DMABUSY)
380 		msleep(10);
381 }
382 
383 static void atmel_lcdfb_start(struct atmel_lcdfb_info *sinfo)
384 {
385 	struct atmel_lcdfb_pdata *pdata = &sinfo->pdata;
386 
387 	lcdc_writel(sinfo, ATMEL_LCDC_DMACON, pdata->default_dmacon);
388 	lcdc_writel(sinfo, ATMEL_LCDC_PWRCON,
389 		(pdata->guard_time << ATMEL_LCDC_GUARDT_OFFSET)
390 		| ATMEL_LCDC_PWR);
391 }
392 
393 static void atmel_lcdfb_update_dma(struct fb_info *info,
394 			       struct fb_var_screeninfo *var)
395 {
396 	struct atmel_lcdfb_info *sinfo = info->par;
397 	struct fb_fix_screeninfo *fix = &info->fix;
398 	unsigned long dma_addr;
399 
400 	dma_addr = (fix->smem_start + var->yoffset * fix->line_length
401 		    + var->xoffset * info->var.bits_per_pixel / 8);
402 
403 	dma_addr &= ~3UL;
404 
405 	/* Set framebuffer DMA base address and pixel offset */
406 	lcdc_writel(sinfo, ATMEL_LCDC_DMABADDR1, dma_addr);
407 
408 	atmel_lcdfb_update_dma2d(sinfo, var, info);
409 }
410 
411 static inline void atmel_lcdfb_free_video_memory(struct atmel_lcdfb_info *sinfo)
412 {
413 	struct fb_info *info = sinfo->info;
414 
415 	dma_free_wc(info->device, info->fix.smem_len, info->screen_base,
416 		    info->fix.smem_start);
417 }
418 
419 /**
420  *	atmel_lcdfb_alloc_video_memory - Allocate framebuffer memory
421  *	@sinfo: the frame buffer to allocate memory for
422  *
423  * 	This function is called only from the atmel_lcdfb_probe()
424  * 	so no locking by fb_info->mm_lock around smem_len setting is needed.
425  */
426 static int atmel_lcdfb_alloc_video_memory(struct atmel_lcdfb_info *sinfo)
427 {
428 	struct fb_info *info = sinfo->info;
429 	struct fb_var_screeninfo *var = &info->var;
430 	unsigned int smem_len;
431 
432 	smem_len = (var->xres_virtual * var->yres_virtual
433 		    * ((var->bits_per_pixel + 7) / 8));
434 	info->fix.smem_len = max(smem_len, sinfo->smem_len);
435 
436 	info->screen_base = dma_alloc_wc(info->device, info->fix.smem_len,
437 					 (dma_addr_t *)&info->fix.smem_start,
438 					 GFP_KERNEL);
439 
440 	if (!info->screen_base) {
441 		return -ENOMEM;
442 	}
443 
444 	memset(info->screen_base, 0, info->fix.smem_len);
445 
446 	return 0;
447 }
448 
449 static const struct fb_videomode *atmel_lcdfb_choose_mode(struct fb_var_screeninfo *var,
450 						     struct fb_info *info)
451 {
452 	struct fb_videomode varfbmode;
453 	const struct fb_videomode *fbmode = NULL;
454 
455 	fb_var_to_videomode(&varfbmode, var);
456 	fbmode = fb_find_nearest_mode(&varfbmode, &info->modelist);
457 	if (fbmode)
458 		fb_videomode_to_var(var, fbmode);
459 	return fbmode;
460 }
461 
462 
463 /**
464  *      atmel_lcdfb_check_var - Validates a var passed in.
465  *      @var: frame buffer variable screen structure
466  *      @info: frame buffer structure that represents a single frame buffer
467  *
468  *	Checks to see if the hardware supports the state requested by
469  *	var passed in. This function does not alter the hardware
470  *	state!!!  This means the data stored in struct fb_info and
471  *	struct atmel_lcdfb_info do not change. This includes the var
472  *	inside of struct fb_info.  Do NOT change these. This function
473  *	can be called on its own if we intent to only test a mode and
474  *	not actually set it. The stuff in modedb.c is a example of
475  *	this. If the var passed in is slightly off by what the
476  *	hardware can support then we alter the var PASSED in to what
477  *	we can do. If the hardware doesn't support mode change a
478  *	-EINVAL will be returned by the upper layers. You don't need
479  *	to implement this function then. If you hardware doesn't
480  *	support changing the resolution then this function is not
481  *	needed. In this case the driver would just provide a var that
482  *	represents the static state the screen is in.
483  *
484  *	Returns negative errno on error, or zero on success.
485  */
486 static int atmel_lcdfb_check_var(struct fb_var_screeninfo *var,
487 			     struct fb_info *info)
488 {
489 	struct device *dev = info->device;
490 	struct atmel_lcdfb_info *sinfo = info->par;
491 	struct atmel_lcdfb_pdata *pdata = &sinfo->pdata;
492 	unsigned long clk_value_khz;
493 
494 	clk_value_khz = clk_get_rate(sinfo->lcdc_clk) / 1000;
495 
496 	dev_dbg(dev, "%s:\n", __func__);
497 
498 	if (!(var->pixclock && var->bits_per_pixel)) {
499 		/* choose a suitable mode if possible */
500 		if (!atmel_lcdfb_choose_mode(var, info)) {
501 			dev_err(dev, "needed value not specified\n");
502 			return -EINVAL;
503 		}
504 	}
505 
506 	dev_dbg(dev, "  resolution: %ux%u\n", var->xres, var->yres);
507 	dev_dbg(dev, "  pixclk:     %lu KHz\n", PICOS2KHZ(var->pixclock));
508 	dev_dbg(dev, "  bpp:        %u\n", var->bits_per_pixel);
509 	dev_dbg(dev, "  clk:        %lu KHz\n", clk_value_khz);
510 
511 	if (PICOS2KHZ(var->pixclock) > clk_value_khz) {
512 		dev_err(dev, "%lu KHz pixel clock is too fast\n", PICOS2KHZ(var->pixclock));
513 		return -EINVAL;
514 	}
515 
516 	/* Do not allow to have real resoulution larger than virtual */
517 	if (var->xres > var->xres_virtual)
518 		var->xres_virtual = var->xres;
519 
520 	if (var->yres > var->yres_virtual)
521 		var->yres_virtual = var->yres;
522 
523 	/* Force same alignment for each line */
524 	var->xres = (var->xres + 3) & ~3UL;
525 	var->xres_virtual = (var->xres_virtual + 3) & ~3UL;
526 
527 	var->red.msb_right = var->green.msb_right = var->blue.msb_right = 0;
528 	var->transp.msb_right = 0;
529 	var->transp.offset = var->transp.length = 0;
530 	var->xoffset = var->yoffset = 0;
531 
532 	if (info->fix.smem_len) {
533 		unsigned int smem_len = (var->xres_virtual * var->yres_virtual
534 					 * ((var->bits_per_pixel + 7) / 8));
535 		if (smem_len > info->fix.smem_len) {
536 			dev_err(dev, "Frame buffer is too small (%u) for screen size (need at least %u)\n",
537 				info->fix.smem_len, smem_len);
538 			return -EINVAL;
539 		}
540 	}
541 
542 	/* Saturate vertical and horizontal timings at maximum values */
543 	var->vsync_len = min_t(u32, var->vsync_len,
544 			(ATMEL_LCDC_VPW >> ATMEL_LCDC_VPW_OFFSET) + 1);
545 	var->upper_margin = min_t(u32, var->upper_margin,
546 			ATMEL_LCDC_VBP >> ATMEL_LCDC_VBP_OFFSET);
547 	var->lower_margin = min_t(u32, var->lower_margin,
548 			ATMEL_LCDC_VFP);
549 	var->right_margin = min_t(u32, var->right_margin,
550 			(ATMEL_LCDC_HFP >> ATMEL_LCDC_HFP_OFFSET) + 1);
551 	var->hsync_len = min_t(u32, var->hsync_len,
552 			(ATMEL_LCDC_HPW >> ATMEL_LCDC_HPW_OFFSET) + 1);
553 	var->left_margin = min_t(u32, var->left_margin,
554 			ATMEL_LCDC_HBP + 1);
555 
556 	/* Some parameters can't be zero */
557 	var->vsync_len = max_t(u32, var->vsync_len, 1);
558 	var->right_margin = max_t(u32, var->right_margin, 1);
559 	var->hsync_len = max_t(u32, var->hsync_len, 1);
560 	var->left_margin = max_t(u32, var->left_margin, 1);
561 
562 	switch (var->bits_per_pixel) {
563 	case 1:
564 	case 2:
565 	case 4:
566 	case 8:
567 		var->red.offset = var->green.offset = var->blue.offset = 0;
568 		var->red.length = var->green.length = var->blue.length
569 			= var->bits_per_pixel;
570 		break;
571 	case 16:
572 		/* Older SOCs use IBGR:555 rather than BGR:565. */
573 		if (sinfo->config->have_intensity_bit)
574 			var->green.length = 5;
575 		else
576 			var->green.length = 6;
577 
578 		if (pdata->lcd_wiring_mode == ATMEL_LCDC_WIRING_RGB) {
579 			/* RGB:5X5 mode */
580 			var->red.offset = var->green.length + 5;
581 			var->blue.offset = 0;
582 		} else {
583 			/* BGR:5X5 mode */
584 			var->red.offset = 0;
585 			var->blue.offset = var->green.length + 5;
586 		}
587 		var->green.offset = 5;
588 		var->red.length = var->blue.length = 5;
589 		break;
590 	case 32:
591 		var->transp.offset = 24;
592 		var->transp.length = 8;
593 		/* fall through */
594 	case 24:
595 		if (pdata->lcd_wiring_mode == ATMEL_LCDC_WIRING_RGB) {
596 			/* RGB:888 mode */
597 			var->red.offset = 16;
598 			var->blue.offset = 0;
599 		} else {
600 			/* BGR:888 mode */
601 			var->red.offset = 0;
602 			var->blue.offset = 16;
603 		}
604 		var->green.offset = 8;
605 		var->red.length = var->green.length = var->blue.length = 8;
606 		break;
607 	default:
608 		dev_err(dev, "color depth %d not supported\n",
609 					var->bits_per_pixel);
610 		return -EINVAL;
611 	}
612 
613 	return 0;
614 }
615 
616 /*
617  * LCD reset sequence
618  */
619 static void atmel_lcdfb_reset(struct atmel_lcdfb_info *sinfo)
620 {
621 	might_sleep();
622 
623 	atmel_lcdfb_stop(sinfo);
624 	atmel_lcdfb_start(sinfo);
625 }
626 
627 /**
628  *      atmel_lcdfb_set_par - Alters the hardware state.
629  *      @info: frame buffer structure that represents a single frame buffer
630  *
631  *	Using the fb_var_screeninfo in fb_info we set the resolution
632  *	of the this particular framebuffer. This function alters the
633  *	par AND the fb_fix_screeninfo stored in fb_info. It doesn't
634  *	not alter var in fb_info since we are using that data. This
635  *	means we depend on the data in var inside fb_info to be
636  *	supported by the hardware.  atmel_lcdfb_check_var is always called
637  *	before atmel_lcdfb_set_par to ensure this.  Again if you can't
638  *	change the resolution you don't need this function.
639  *
640  */
641 static int atmel_lcdfb_set_par(struct fb_info *info)
642 {
643 	struct atmel_lcdfb_info *sinfo = info->par;
644 	struct atmel_lcdfb_pdata *pdata = &sinfo->pdata;
645 	unsigned long hozval_linesz;
646 	unsigned long value;
647 	unsigned long clk_value_khz;
648 	unsigned long bits_per_line;
649 	unsigned long pix_factor = 2;
650 
651 	might_sleep();
652 
653 	dev_dbg(info->device, "%s:\n", __func__);
654 	dev_dbg(info->device, "  * resolution: %ux%u (%ux%u virtual)\n",
655 		 info->var.xres, info->var.yres,
656 		 info->var.xres_virtual, info->var.yres_virtual);
657 
658 	atmel_lcdfb_stop_nowait(sinfo);
659 
660 	if (info->var.bits_per_pixel == 1)
661 		info->fix.visual = FB_VISUAL_MONO01;
662 	else if (info->var.bits_per_pixel <= 8)
663 		info->fix.visual = FB_VISUAL_PSEUDOCOLOR;
664 	else
665 		info->fix.visual = FB_VISUAL_TRUECOLOR;
666 
667 	bits_per_line = info->var.xres_virtual * info->var.bits_per_pixel;
668 	info->fix.line_length = DIV_ROUND_UP(bits_per_line, 8);
669 
670 	/* Re-initialize the DMA engine... */
671 	dev_dbg(info->device, "  * update DMA engine\n");
672 	atmel_lcdfb_update_dma(info, &info->var);
673 
674 	/* ...set frame size and burst length = 8 words (?) */
675 	value = (info->var.yres * info->var.xres * info->var.bits_per_pixel) / 32;
676 	value |= ((ATMEL_LCDC_DMA_BURST_LEN - 1) << ATMEL_LCDC_BLENGTH_OFFSET);
677 	lcdc_writel(sinfo, ATMEL_LCDC_DMAFRMCFG, value);
678 
679 	/* Now, the LCDC core... */
680 
681 	/* Set pixel clock */
682 	if (sinfo->config->have_alt_pixclock)
683 		pix_factor = 1;
684 
685 	clk_value_khz = clk_get_rate(sinfo->lcdc_clk) / 1000;
686 
687 	value = DIV_ROUND_UP(clk_value_khz, PICOS2KHZ(info->var.pixclock));
688 
689 	if (value < pix_factor) {
690 		dev_notice(info->device, "Bypassing pixel clock divider\n");
691 		lcdc_writel(sinfo, ATMEL_LCDC_LCDCON1, ATMEL_LCDC_BYPASS);
692 	} else {
693 		value = (value / pix_factor) - 1;
694 		dev_dbg(info->device, "  * programming CLKVAL = 0x%08lx\n",
695 				value);
696 		lcdc_writel(sinfo, ATMEL_LCDC_LCDCON1,
697 				value << ATMEL_LCDC_CLKVAL_OFFSET);
698 		info->var.pixclock =
699 			KHZ2PICOS(clk_value_khz / (pix_factor * (value + 1)));
700 		dev_dbg(info->device, "  updated pixclk:     %lu KHz\n",
701 					PICOS2KHZ(info->var.pixclock));
702 	}
703 
704 
705 	/* Initialize control register 2 */
706 	value = pdata->default_lcdcon2;
707 
708 	if (!(info->var.sync & FB_SYNC_HOR_HIGH_ACT))
709 		value |= ATMEL_LCDC_INVLINE_INVERTED;
710 	if (!(info->var.sync & FB_SYNC_VERT_HIGH_ACT))
711 		value |= ATMEL_LCDC_INVFRAME_INVERTED;
712 
713 	switch (info->var.bits_per_pixel) {
714 		case 1:	value |= ATMEL_LCDC_PIXELSIZE_1; break;
715 		case 2: value |= ATMEL_LCDC_PIXELSIZE_2; break;
716 		case 4: value |= ATMEL_LCDC_PIXELSIZE_4; break;
717 		case 8: value |= ATMEL_LCDC_PIXELSIZE_8; break;
718 		case 15: /* fall through */
719 		case 16: value |= ATMEL_LCDC_PIXELSIZE_16; break;
720 		case 24: value |= ATMEL_LCDC_PIXELSIZE_24; break;
721 		case 32: value |= ATMEL_LCDC_PIXELSIZE_32; break;
722 		default: BUG(); break;
723 	}
724 	dev_dbg(info->device, "  * LCDCON2 = %08lx\n", value);
725 	lcdc_writel(sinfo, ATMEL_LCDC_LCDCON2, value);
726 
727 	/* Vertical timing */
728 	value = (info->var.vsync_len - 1) << ATMEL_LCDC_VPW_OFFSET;
729 	value |= info->var.upper_margin << ATMEL_LCDC_VBP_OFFSET;
730 	value |= info->var.lower_margin;
731 	dev_dbg(info->device, "  * LCDTIM1 = %08lx\n", value);
732 	lcdc_writel(sinfo, ATMEL_LCDC_TIM1, value);
733 
734 	/* Horizontal timing */
735 	value = (info->var.right_margin - 1) << ATMEL_LCDC_HFP_OFFSET;
736 	value |= (info->var.hsync_len - 1) << ATMEL_LCDC_HPW_OFFSET;
737 	value |= (info->var.left_margin - 1);
738 	dev_dbg(info->device, "  * LCDTIM2 = %08lx\n", value);
739 	lcdc_writel(sinfo, ATMEL_LCDC_TIM2, value);
740 
741 	/* Horizontal value (aka line size) */
742 	hozval_linesz = compute_hozval(sinfo, info->var.xres);
743 
744 	/* Display size */
745 	value = (hozval_linesz - 1) << ATMEL_LCDC_HOZVAL_OFFSET;
746 	value |= info->var.yres - 1;
747 	dev_dbg(info->device, "  * LCDFRMCFG = %08lx\n", value);
748 	lcdc_writel(sinfo, ATMEL_LCDC_LCDFRMCFG, value);
749 
750 	/* FIFO Threshold: Use formula from data sheet */
751 	value = ATMEL_LCDC_FIFO_SIZE - (2 * ATMEL_LCDC_DMA_BURST_LEN + 3);
752 	lcdc_writel(sinfo, ATMEL_LCDC_FIFO, value);
753 
754 	/* Toggle LCD_MODE every frame */
755 	lcdc_writel(sinfo, ATMEL_LCDC_MVAL, 0);
756 
757 	/* Disable all interrupts */
758 	lcdc_writel(sinfo, ATMEL_LCDC_IDR, ~0UL);
759 	/* Enable FIFO & DMA errors */
760 	lcdc_writel(sinfo, ATMEL_LCDC_IER, ATMEL_LCDC_UFLWI | ATMEL_LCDC_OWRI | ATMEL_LCDC_MERI);
761 
762 	/* ...wait for DMA engine to become idle... */
763 	while (lcdc_readl(sinfo, ATMEL_LCDC_DMACON) & ATMEL_LCDC_DMABUSY)
764 		msleep(10);
765 
766 	atmel_lcdfb_start(sinfo);
767 
768 	dev_dbg(info->device, "  * DONE\n");
769 
770 	return 0;
771 }
772 
773 static inline unsigned int chan_to_field(unsigned int chan, const struct fb_bitfield *bf)
774 {
775 	chan &= 0xffff;
776 	chan >>= 16 - bf->length;
777 	return chan << bf->offset;
778 }
779 
780 /**
781  *  	atmel_lcdfb_setcolreg - Optional function. Sets a color register.
782  *      @regno: Which register in the CLUT we are programming
783  *      @red: The red value which can be up to 16 bits wide
784  *	@green: The green value which can be up to 16 bits wide
785  *	@blue:  The blue value which can be up to 16 bits wide.
786  *	@transp: If supported the alpha value which can be up to 16 bits wide.
787  *      @info: frame buffer info structure
788  *
789  *  	Set a single color register. The values supplied have a 16 bit
790  *  	magnitude which needs to be scaled in this function for the hardware.
791  *	Things to take into consideration are how many color registers, if
792  *	any, are supported with the current color visual. With truecolor mode
793  *	no color palettes are supported. Here a pseudo palette is created
794  *	which we store the value in pseudo_palette in struct fb_info. For
795  *	pseudocolor mode we have a limited color palette. To deal with this
796  *	we can program what color is displayed for a particular pixel value.
797  *	DirectColor is similar in that we can program each color field. If
798  *	we have a static colormap we don't need to implement this function.
799  *
800  *	Returns negative errno on error, or zero on success. In an
801  *	ideal world, this would have been the case, but as it turns
802  *	out, the other drivers return 1 on failure, so that's what
803  *	we're going to do.
804  */
805 static int atmel_lcdfb_setcolreg(unsigned int regno, unsigned int red,
806 			     unsigned int green, unsigned int blue,
807 			     unsigned int transp, struct fb_info *info)
808 {
809 	struct atmel_lcdfb_info *sinfo = info->par;
810 	struct atmel_lcdfb_pdata *pdata = &sinfo->pdata;
811 	unsigned int val;
812 	u32 *pal;
813 	int ret = 1;
814 
815 	if (info->var.grayscale)
816 		red = green = blue = (19595 * red + 38470 * green
817 				      + 7471 * blue) >> 16;
818 
819 	switch (info->fix.visual) {
820 	case FB_VISUAL_TRUECOLOR:
821 		if (regno < 16) {
822 			pal = info->pseudo_palette;
823 
824 			val  = chan_to_field(red, &info->var.red);
825 			val |= chan_to_field(green, &info->var.green);
826 			val |= chan_to_field(blue, &info->var.blue);
827 
828 			pal[regno] = val;
829 			ret = 0;
830 		}
831 		break;
832 
833 	case FB_VISUAL_PSEUDOCOLOR:
834 		if (regno < 256) {
835 			if (sinfo->config->have_intensity_bit) {
836 				/* old style I+BGR:555 */
837 				val  = ((red   >> 11) & 0x001f);
838 				val |= ((green >>  6) & 0x03e0);
839 				val |= ((blue  >>  1) & 0x7c00);
840 
841 				/*
842 				 * TODO: intensity bit. Maybe something like
843 				 *   ~(red[10] ^ green[10] ^ blue[10]) & 1
844 				 */
845 			} else {
846 				/* new style BGR:565 / RGB:565 */
847 				if (pdata->lcd_wiring_mode == ATMEL_LCDC_WIRING_RGB) {
848 					val  = ((blue >> 11) & 0x001f);
849 					val |= ((red  >>  0) & 0xf800);
850 				} else {
851 					val  = ((red  >> 11) & 0x001f);
852 					val |= ((blue >>  0) & 0xf800);
853 				}
854 
855 				val |= ((green >>  5) & 0x07e0);
856 			}
857 
858 			lcdc_writel(sinfo, ATMEL_LCDC_LUT(regno), val);
859 			ret = 0;
860 		}
861 		break;
862 
863 	case FB_VISUAL_MONO01:
864 		if (regno < 2) {
865 			val = (regno == 0) ? 0x00 : 0x1F;
866 			lcdc_writel(sinfo, ATMEL_LCDC_LUT(regno), val);
867 			ret = 0;
868 		}
869 		break;
870 
871 	}
872 
873 	return ret;
874 }
875 
876 static int atmel_lcdfb_pan_display(struct fb_var_screeninfo *var,
877 			       struct fb_info *info)
878 {
879 	dev_dbg(info->device, "%s\n", __func__);
880 
881 	atmel_lcdfb_update_dma(info, var);
882 
883 	return 0;
884 }
885 
886 static int atmel_lcdfb_blank(int blank_mode, struct fb_info *info)
887 {
888 	struct atmel_lcdfb_info *sinfo = info->par;
889 
890 	switch (blank_mode) {
891 	case FB_BLANK_UNBLANK:
892 	case FB_BLANK_NORMAL:
893 		atmel_lcdfb_start(sinfo);
894 		break;
895 	case FB_BLANK_VSYNC_SUSPEND:
896 	case FB_BLANK_HSYNC_SUSPEND:
897 		break;
898 	case FB_BLANK_POWERDOWN:
899 		atmel_lcdfb_stop(sinfo);
900 		break;
901 	default:
902 		return -EINVAL;
903 	}
904 
905 	/* let fbcon do a soft blank for us */
906 	return ((blank_mode == FB_BLANK_NORMAL) ? 1 : 0);
907 }
908 
909 static struct fb_ops atmel_lcdfb_ops = {
910 	.owner		= THIS_MODULE,
911 	.fb_check_var	= atmel_lcdfb_check_var,
912 	.fb_set_par	= atmel_lcdfb_set_par,
913 	.fb_setcolreg	= atmel_lcdfb_setcolreg,
914 	.fb_blank	= atmel_lcdfb_blank,
915 	.fb_pan_display	= atmel_lcdfb_pan_display,
916 	.fb_fillrect	= cfb_fillrect,
917 	.fb_copyarea	= cfb_copyarea,
918 	.fb_imageblit	= cfb_imageblit,
919 };
920 
921 static irqreturn_t atmel_lcdfb_interrupt(int irq, void *dev_id)
922 {
923 	struct fb_info *info = dev_id;
924 	struct atmel_lcdfb_info *sinfo = info->par;
925 	u32 status;
926 
927 	status = lcdc_readl(sinfo, ATMEL_LCDC_ISR);
928 	if (status & ATMEL_LCDC_UFLWI) {
929 		dev_warn(info->device, "FIFO underflow %#x\n", status);
930 		/* reset DMA and FIFO to avoid screen shifting */
931 		schedule_work(&sinfo->task);
932 	}
933 	lcdc_writel(sinfo, ATMEL_LCDC_ICR, status);
934 	return IRQ_HANDLED;
935 }
936 
937 /*
938  * LCD controller task (to reset the LCD)
939  */
940 static void atmel_lcdfb_task(struct work_struct *work)
941 {
942 	struct atmel_lcdfb_info *sinfo =
943 		container_of(work, struct atmel_lcdfb_info, task);
944 
945 	atmel_lcdfb_reset(sinfo);
946 }
947 
948 static int __init atmel_lcdfb_init_fbinfo(struct atmel_lcdfb_info *sinfo)
949 {
950 	struct fb_info *info = sinfo->info;
951 	int ret = 0;
952 
953 	info->var.activate |= FB_ACTIVATE_FORCE | FB_ACTIVATE_NOW;
954 
955 	dev_info(info->device,
956 	       "%luKiB frame buffer at %08lx (mapped at %p)\n",
957 	       (unsigned long)info->fix.smem_len / 1024,
958 	       (unsigned long)info->fix.smem_start,
959 	       info->screen_base);
960 
961 	/* Allocate colormap */
962 	ret = fb_alloc_cmap(&info->cmap, 256, 0);
963 	if (ret < 0)
964 		dev_err(info->device, "Alloc color map failed\n");
965 
966 	return ret;
967 }
968 
969 static void atmel_lcdfb_start_clock(struct atmel_lcdfb_info *sinfo)
970 {
971 	clk_prepare_enable(sinfo->bus_clk);
972 	clk_prepare_enable(sinfo->lcdc_clk);
973 }
974 
975 static void atmel_lcdfb_stop_clock(struct atmel_lcdfb_info *sinfo)
976 {
977 	clk_disable_unprepare(sinfo->bus_clk);
978 	clk_disable_unprepare(sinfo->lcdc_clk);
979 }
980 
981 #ifdef CONFIG_OF
982 static const struct of_device_id atmel_lcdfb_dt_ids[] = {
983 	{ .compatible = "atmel,at91sam9261-lcdc" , .data = &at91sam9261_config, },
984 	{ .compatible = "atmel,at91sam9263-lcdc" , .data = &at91sam9263_config, },
985 	{ .compatible = "atmel,at91sam9g10-lcdc" , .data = &at91sam9g10_config, },
986 	{ .compatible = "atmel,at91sam9g45-lcdc" , .data = &at91sam9g45_config, },
987 	{ .compatible = "atmel,at91sam9g45es-lcdc" , .data = &at91sam9g45es_config, },
988 	{ .compatible = "atmel,at91sam9rl-lcdc" , .data = &at91sam9rl_config, },
989 	{ .compatible = "atmel,at32ap-lcdc" , .data = &at32ap_config, },
990 	{ /* sentinel */ }
991 };
992 
993 MODULE_DEVICE_TABLE(of, atmel_lcdfb_dt_ids);
994 
995 static const char *atmel_lcdfb_wiring_modes[] = {
996 	[ATMEL_LCDC_WIRING_BGR]	= "BRG",
997 	[ATMEL_LCDC_WIRING_RGB]	= "RGB",
998 };
999 
1000 static int atmel_lcdfb_get_of_wiring_modes(struct device_node *np)
1001 {
1002 	const char *mode;
1003 	int err, i;
1004 
1005 	err = of_property_read_string(np, "atmel,lcd-wiring-mode", &mode);
1006 	if (err < 0)
1007 		return ATMEL_LCDC_WIRING_BGR;
1008 
1009 	for (i = 0; i < ARRAY_SIZE(atmel_lcdfb_wiring_modes); i++)
1010 		if (!strcasecmp(mode, atmel_lcdfb_wiring_modes[i]))
1011 			return i;
1012 
1013 	return -ENODEV;
1014 }
1015 
1016 static void atmel_lcdfb_power_control_gpio(struct atmel_lcdfb_pdata *pdata, int on)
1017 {
1018 	struct atmel_lcdfb_power_ctrl_gpio *og;
1019 
1020 	list_for_each_entry(og, &pdata->pwr_gpios, list)
1021 		gpiod_set_value(og->gpiod, on);
1022 }
1023 
1024 static int atmel_lcdfb_of_init(struct atmel_lcdfb_info *sinfo)
1025 {
1026 	struct fb_info *info = sinfo->info;
1027 	struct atmel_lcdfb_pdata *pdata = &sinfo->pdata;
1028 	struct fb_var_screeninfo *var = &info->var;
1029 	struct device *dev = &sinfo->pdev->dev;
1030 	struct device_node *np =dev->of_node;
1031 	struct device_node *display_np;
1032 	struct atmel_lcdfb_power_ctrl_gpio *og;
1033 	bool is_gpio_power = false;
1034 	struct fb_videomode fb_vm;
1035 	struct gpio_desc *gpiod;
1036 	struct videomode vm;
1037 	int ret = -ENOENT;
1038 	int i;
1039 
1040 	sinfo->config = (struct atmel_lcdfb_config*)
1041 		of_match_device(atmel_lcdfb_dt_ids, dev)->data;
1042 
1043 	display_np = of_parse_phandle(np, "display", 0);
1044 	if (!display_np) {
1045 		dev_err(dev, "failed to find display phandle\n");
1046 		return -ENOENT;
1047 	}
1048 
1049 	ret = of_property_read_u32(display_np, "bits-per-pixel", &var->bits_per_pixel);
1050 	if (ret < 0) {
1051 		dev_err(dev, "failed to get property bits-per-pixel\n");
1052 		goto put_display_node;
1053 	}
1054 
1055 	ret = of_property_read_u32(display_np, "atmel,guard-time", &pdata->guard_time);
1056 	if (ret < 0) {
1057 		dev_err(dev, "failed to get property atmel,guard-time\n");
1058 		goto put_display_node;
1059 	}
1060 
1061 	ret = of_property_read_u32(display_np, "atmel,lcdcon2", &pdata->default_lcdcon2);
1062 	if (ret < 0) {
1063 		dev_err(dev, "failed to get property atmel,lcdcon2\n");
1064 		goto put_display_node;
1065 	}
1066 
1067 	ret = of_property_read_u32(display_np, "atmel,dmacon", &pdata->default_dmacon);
1068 	if (ret < 0) {
1069 		dev_err(dev, "failed to get property bits-per-pixel\n");
1070 		goto put_display_node;
1071 	}
1072 
1073 	INIT_LIST_HEAD(&pdata->pwr_gpios);
1074 	ret = -ENOMEM;
1075 	for (i = 0; i < gpiod_count(dev, "atmel,power-control"); i++) {
1076 		gpiod = devm_gpiod_get_index(dev, "atmel,power-control",
1077 					     i, GPIOD_ASIS);
1078 		if (IS_ERR(gpiod))
1079 			continue;
1080 
1081 		og = devm_kzalloc(dev, sizeof(*og), GFP_KERNEL);
1082 		if (!og)
1083 			goto put_display_node;
1084 
1085 		og->gpiod = gpiod;
1086 		is_gpio_power = true;
1087 
1088 		ret = gpiod_direction_output(gpiod, gpiod_is_active_low(gpiod));
1089 		if (ret) {
1090 			dev_err(dev, "set direction output gpio atmel,power-control[%d] failed\n", i);
1091 			goto put_display_node;
1092 		}
1093 		list_add(&og->list, &pdata->pwr_gpios);
1094 	}
1095 
1096 	if (is_gpio_power)
1097 		pdata->atmel_lcdfb_power_control = atmel_lcdfb_power_control_gpio;
1098 
1099 	ret = atmel_lcdfb_get_of_wiring_modes(display_np);
1100 	if (ret < 0) {
1101 		dev_err(dev, "invalid atmel,lcd-wiring-mode\n");
1102 		goto put_display_node;
1103 	}
1104 	pdata->lcd_wiring_mode = ret;
1105 
1106 	pdata->lcdcon_is_backlight = of_property_read_bool(display_np, "atmel,lcdcon-backlight");
1107 	pdata->lcdcon_pol_negative = of_property_read_bool(display_np, "atmel,lcdcon-backlight-inverted");
1108 
1109 	ret = of_get_videomode(display_np, &vm, OF_USE_NATIVE_MODE);
1110 	if (ret) {
1111 		dev_err(dev, "failed to get videomode from DT\n");
1112 		goto put_display_node;
1113 	}
1114 
1115 	ret = fb_videomode_from_videomode(&vm, &fb_vm);
1116 	if (ret < 0)
1117 		goto put_display_node;
1118 
1119 	fb_add_videomode(&fb_vm, &info->modelist);
1120 
1121 put_display_node:
1122 	of_node_put(display_np);
1123 	return ret;
1124 }
1125 #else
1126 static int atmel_lcdfb_of_init(struct atmel_lcdfb_info *sinfo)
1127 {
1128 	return 0;
1129 }
1130 #endif
1131 
1132 static int __init atmel_lcdfb_probe(struct platform_device *pdev)
1133 {
1134 	struct device *dev = &pdev->dev;
1135 	struct fb_info *info;
1136 	struct atmel_lcdfb_info *sinfo;
1137 	struct atmel_lcdfb_pdata *pdata = NULL;
1138 	struct resource *regs = NULL;
1139 	struct resource *map = NULL;
1140 	struct fb_modelist *modelist;
1141 	int ret;
1142 
1143 	dev_dbg(dev, "%s BEGIN\n", __func__);
1144 
1145 	ret = -ENOMEM;
1146 	info = framebuffer_alloc(sizeof(struct atmel_lcdfb_info), dev);
1147 	if (!info) {
1148 		dev_err(dev, "cannot allocate memory\n");
1149 		goto out;
1150 	}
1151 
1152 	sinfo = info->par;
1153 	sinfo->pdev = pdev;
1154 	sinfo->info = info;
1155 
1156 	INIT_LIST_HEAD(&info->modelist);
1157 
1158 	if (pdev->dev.of_node) {
1159 		ret = atmel_lcdfb_of_init(sinfo);
1160 		if (ret)
1161 			goto free_info;
1162 	} else if (dev_get_platdata(dev)) {
1163 		struct fb_monspecs *monspecs;
1164 		int i;
1165 
1166 		pdata = dev_get_platdata(dev);
1167 		monspecs = pdata->default_monspecs;
1168 		sinfo->pdata = *pdata;
1169 
1170 		for (i = 0; i < monspecs->modedb_len; i++)
1171 			fb_add_videomode(&monspecs->modedb[i], &info->modelist);
1172 
1173 		sinfo->config = atmel_lcdfb_get_config(pdev);
1174 
1175 		info->var.bits_per_pixel = pdata->default_bpp ? pdata->default_bpp : 16;
1176 		memcpy(&info->monspecs, pdata->default_monspecs, sizeof(info->monspecs));
1177 	} else {
1178 		dev_err(dev, "cannot get default configuration\n");
1179 		goto free_info;
1180 	}
1181 
1182 	if (!sinfo->config)
1183 		goto free_info;
1184 
1185 	sinfo->reg_lcd = devm_regulator_get(&pdev->dev, "lcd");
1186 	if (IS_ERR(sinfo->reg_lcd))
1187 		sinfo->reg_lcd = NULL;
1188 
1189 	info->flags = ATMEL_LCDFB_FBINFO_DEFAULT;
1190 	info->pseudo_palette = sinfo->pseudo_palette;
1191 	info->fbops = &atmel_lcdfb_ops;
1192 
1193 	info->fix = atmel_lcdfb_fix;
1194 	strcpy(info->fix.id, sinfo->pdev->name);
1195 
1196 	/* Enable LCDC Clocks */
1197 	sinfo->bus_clk = clk_get(dev, "hclk");
1198 	if (IS_ERR(sinfo->bus_clk)) {
1199 		ret = PTR_ERR(sinfo->bus_clk);
1200 		goto free_info;
1201 	}
1202 	sinfo->lcdc_clk = clk_get(dev, "lcdc_clk");
1203 	if (IS_ERR(sinfo->lcdc_clk)) {
1204 		ret = PTR_ERR(sinfo->lcdc_clk);
1205 		goto put_bus_clk;
1206 	}
1207 	atmel_lcdfb_start_clock(sinfo);
1208 
1209 	modelist = list_first_entry(&info->modelist,
1210 			struct fb_modelist, list);
1211 	fb_videomode_to_var(&info->var, &modelist->mode);
1212 
1213 	atmel_lcdfb_check_var(&info->var, info);
1214 
1215 	regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
1216 	if (!regs) {
1217 		dev_err(dev, "resources unusable\n");
1218 		ret = -ENXIO;
1219 		goto stop_clk;
1220 	}
1221 
1222 	sinfo->irq_base = platform_get_irq(pdev, 0);
1223 	if (sinfo->irq_base < 0) {
1224 		dev_err(dev, "unable to get irq\n");
1225 		ret = sinfo->irq_base;
1226 		goto stop_clk;
1227 	}
1228 
1229 	/* Initialize video memory */
1230 	map = platform_get_resource(pdev, IORESOURCE_MEM, 1);
1231 	if (map) {
1232 		/* use a pre-allocated memory buffer */
1233 		info->fix.smem_start = map->start;
1234 		info->fix.smem_len = resource_size(map);
1235 		if (!request_mem_region(info->fix.smem_start,
1236 					info->fix.smem_len, pdev->name)) {
1237 			ret = -EBUSY;
1238 			goto stop_clk;
1239 		}
1240 
1241 		info->screen_base = ioremap_wc(info->fix.smem_start,
1242 					       info->fix.smem_len);
1243 		if (!info->screen_base) {
1244 			ret = -ENOMEM;
1245 			goto release_intmem;
1246 		}
1247 
1248 		/*
1249 		 * Don't clear the framebuffer -- someone may have set
1250 		 * up a splash image.
1251 		 */
1252 	} else {
1253 		/* allocate memory buffer */
1254 		ret = atmel_lcdfb_alloc_video_memory(sinfo);
1255 		if (ret < 0) {
1256 			dev_err(dev, "cannot allocate framebuffer: %d\n", ret);
1257 			goto stop_clk;
1258 		}
1259 	}
1260 
1261 	/* LCDC registers */
1262 	info->fix.mmio_start = regs->start;
1263 	info->fix.mmio_len = resource_size(regs);
1264 
1265 	if (!request_mem_region(info->fix.mmio_start,
1266 				info->fix.mmio_len, pdev->name)) {
1267 		ret = -EBUSY;
1268 		goto free_fb;
1269 	}
1270 
1271 	sinfo->mmio = ioremap(info->fix.mmio_start, info->fix.mmio_len);
1272 	if (!sinfo->mmio) {
1273 		dev_err(dev, "cannot map LCDC registers\n");
1274 		ret = -ENOMEM;
1275 		goto release_mem;
1276 	}
1277 
1278 	/* Initialize PWM for contrast or backlight ("off") */
1279 	init_contrast(sinfo);
1280 
1281 	/* interrupt */
1282 	ret = request_irq(sinfo->irq_base, atmel_lcdfb_interrupt, 0, pdev->name, info);
1283 	if (ret) {
1284 		dev_err(dev, "request_irq failed: %d\n", ret);
1285 		goto unmap_mmio;
1286 	}
1287 
1288 	/* Some operations on the LCDC might sleep and
1289 	 * require a preemptible task context */
1290 	INIT_WORK(&sinfo->task, atmel_lcdfb_task);
1291 
1292 	ret = atmel_lcdfb_init_fbinfo(sinfo);
1293 	if (ret < 0) {
1294 		dev_err(dev, "init fbinfo failed: %d\n", ret);
1295 		goto unregister_irqs;
1296 	}
1297 
1298 	ret = atmel_lcdfb_set_par(info);
1299 	if (ret < 0) {
1300 		dev_err(dev, "set par failed: %d\n", ret);
1301 		goto unregister_irqs;
1302 	}
1303 
1304 	dev_set_drvdata(dev, info);
1305 
1306 	/*
1307 	 * Tell the world that we're ready to go
1308 	 */
1309 	ret = register_framebuffer(info);
1310 	if (ret < 0) {
1311 		dev_err(dev, "failed to register framebuffer device: %d\n", ret);
1312 		goto reset_drvdata;
1313 	}
1314 
1315 	/* Power up the LCDC screen */
1316 	atmel_lcdfb_power_control(sinfo, 1);
1317 
1318 	dev_info(dev, "fb%d: Atmel LCDC at 0x%08lx (mapped at %p), irq %d\n",
1319 		       info->node, info->fix.mmio_start, sinfo->mmio, sinfo->irq_base);
1320 
1321 	return 0;
1322 
1323 reset_drvdata:
1324 	dev_set_drvdata(dev, NULL);
1325 	fb_dealloc_cmap(&info->cmap);
1326 unregister_irqs:
1327 	cancel_work_sync(&sinfo->task);
1328 	free_irq(sinfo->irq_base, info);
1329 unmap_mmio:
1330 	exit_backlight(sinfo);
1331 	iounmap(sinfo->mmio);
1332 release_mem:
1333  	release_mem_region(info->fix.mmio_start, info->fix.mmio_len);
1334 free_fb:
1335 	if (map)
1336 		iounmap(info->screen_base);
1337 	else
1338 		atmel_lcdfb_free_video_memory(sinfo);
1339 
1340 release_intmem:
1341 	if (map)
1342 		release_mem_region(info->fix.smem_start, info->fix.smem_len);
1343 stop_clk:
1344 	atmel_lcdfb_stop_clock(sinfo);
1345 	clk_put(sinfo->lcdc_clk);
1346 put_bus_clk:
1347 	clk_put(sinfo->bus_clk);
1348 free_info:
1349 	framebuffer_release(info);
1350 out:
1351 	dev_dbg(dev, "%s FAILED\n", __func__);
1352 	return ret;
1353 }
1354 
1355 static int __exit atmel_lcdfb_remove(struct platform_device *pdev)
1356 {
1357 	struct device *dev = &pdev->dev;
1358 	struct fb_info *info = dev_get_drvdata(dev);
1359 	struct atmel_lcdfb_info *sinfo;
1360 	struct atmel_lcdfb_pdata *pdata;
1361 
1362 	if (!info || !info->par)
1363 		return 0;
1364 	sinfo = info->par;
1365 	pdata = &sinfo->pdata;
1366 
1367 	cancel_work_sync(&sinfo->task);
1368 	exit_backlight(sinfo);
1369 	atmel_lcdfb_power_control(sinfo, 0);
1370 	unregister_framebuffer(info);
1371 	atmel_lcdfb_stop_clock(sinfo);
1372 	clk_put(sinfo->lcdc_clk);
1373 	clk_put(sinfo->bus_clk);
1374 	fb_dealloc_cmap(&info->cmap);
1375 	free_irq(sinfo->irq_base, info);
1376 	iounmap(sinfo->mmio);
1377  	release_mem_region(info->fix.mmio_start, info->fix.mmio_len);
1378 	if (platform_get_resource(pdev, IORESOURCE_MEM, 1)) {
1379 		iounmap(info->screen_base);
1380 		release_mem_region(info->fix.smem_start, info->fix.smem_len);
1381 	} else {
1382 		atmel_lcdfb_free_video_memory(sinfo);
1383 	}
1384 
1385 	framebuffer_release(info);
1386 
1387 	return 0;
1388 }
1389 
1390 #ifdef CONFIG_PM
1391 
1392 static int atmel_lcdfb_suspend(struct platform_device *pdev, pm_message_t mesg)
1393 {
1394 	struct fb_info *info = platform_get_drvdata(pdev);
1395 	struct atmel_lcdfb_info *sinfo = info->par;
1396 
1397 	/*
1398 	 * We don't want to handle interrupts while the clock is
1399 	 * stopped. It may take forever.
1400 	 */
1401 	lcdc_writel(sinfo, ATMEL_LCDC_IDR, ~0UL);
1402 
1403 	sinfo->saved_lcdcon = lcdc_readl(sinfo, ATMEL_LCDC_CONTRAST_CTR);
1404 	lcdc_writel(sinfo, ATMEL_LCDC_CONTRAST_CTR, 0);
1405 	atmel_lcdfb_power_control(sinfo, 0);
1406 	atmel_lcdfb_stop(sinfo);
1407 	atmel_lcdfb_stop_clock(sinfo);
1408 
1409 	return 0;
1410 }
1411 
1412 static int atmel_lcdfb_resume(struct platform_device *pdev)
1413 {
1414 	struct fb_info *info = platform_get_drvdata(pdev);
1415 	struct atmel_lcdfb_info *sinfo = info->par;
1416 
1417 	atmel_lcdfb_start_clock(sinfo);
1418 	atmel_lcdfb_start(sinfo);
1419 	atmel_lcdfb_power_control(sinfo, 1);
1420 	lcdc_writel(sinfo, ATMEL_LCDC_CONTRAST_CTR, sinfo->saved_lcdcon);
1421 
1422 	/* Enable FIFO & DMA errors */
1423 	lcdc_writel(sinfo, ATMEL_LCDC_IER, ATMEL_LCDC_UFLWI
1424 			| ATMEL_LCDC_OWRI | ATMEL_LCDC_MERI);
1425 
1426 	return 0;
1427 }
1428 
1429 #else
1430 #define atmel_lcdfb_suspend	NULL
1431 #define atmel_lcdfb_resume	NULL
1432 #endif
1433 
1434 static struct platform_driver atmel_lcdfb_driver = {
1435 	.remove		= __exit_p(atmel_lcdfb_remove),
1436 	.suspend	= atmel_lcdfb_suspend,
1437 	.resume		= atmel_lcdfb_resume,
1438 	.id_table	= atmel_lcdfb_devtypes,
1439 	.driver		= {
1440 		.name	= "atmel_lcdfb",
1441 		.of_match_table	= of_match_ptr(atmel_lcdfb_dt_ids),
1442 	},
1443 };
1444 
1445 module_platform_driver_probe(atmel_lcdfb_driver, atmel_lcdfb_probe);
1446 
1447 MODULE_DESCRIPTION("AT91/AT32 LCD Controller framebuffer driver");
1448 MODULE_AUTHOR("Nicolas Ferre <nicolas.ferre@atmel.com>");
1449 MODULE_LICENSE("GPL");
1450