xref: /linux/drivers/video/fbdev/mmp/fb/mmpfb.c (revision c17ee635fd3a482b2ad2bf5e269755c2eae5f25e)
1 // SPDX-License-Identifier: GPL-2.0-or-later
2 /*
3  * linux/drivers/video/mmp/fb/mmpfb.c
4  * Framebuffer driver for Marvell Display controller.
5  *
6  * Copyright (C) 2012 Marvell Technology Group Ltd.
7  * Authors: Zhou Zhu <zzhu3@marvell.com>
8  */
9 #include <linux/module.h>
10 #include <linux/dma-mapping.h>
11 #include <linux/platform_device.h>
12 #include "mmpfb.h"
13 
14 static int var_to_pixfmt(struct fb_var_screeninfo *var)
15 {
16 	/*
17 	 * Pseudocolor mode?
18 	 */
19 	if (var->bits_per_pixel == 8)
20 		return PIXFMT_PSEUDOCOLOR;
21 
22 	/*
23 	 * Check for YUV422PLANAR.
24 	 */
25 	if (var->bits_per_pixel == 16 && var->red.length == 8 &&
26 			var->green.length == 4 && var->blue.length == 4) {
27 		if (var->green.offset >= var->blue.offset)
28 			return PIXFMT_YUV422P;
29 		else
30 			return PIXFMT_YVU422P;
31 	}
32 
33 	/*
34 	 * Check for YUV420PLANAR.
35 	 */
36 	if (var->bits_per_pixel == 12 && var->red.length == 8 &&
37 			var->green.length == 2 && var->blue.length == 2) {
38 		if (var->green.offset >= var->blue.offset)
39 			return PIXFMT_YUV420P;
40 		else
41 			return PIXFMT_YVU420P;
42 	}
43 
44 	/*
45 	 * Check for YUV422PACK.
46 	 */
47 	if (var->bits_per_pixel == 16 && var->red.length == 16 &&
48 			var->green.length == 16 && var->blue.length == 16) {
49 		if (var->red.offset == 0)
50 			return PIXFMT_YUYV;
51 		else if (var->green.offset >= var->blue.offset)
52 			return PIXFMT_UYVY;
53 		else
54 			return PIXFMT_VYUY;
55 	}
56 
57 	/*
58 	 * Check for 565/1555.
59 	 */
60 	if (var->bits_per_pixel == 16 && var->red.length <= 5 &&
61 			var->green.length <= 6 && var->blue.length <= 5) {
62 		if (var->transp.length == 0) {
63 			if (var->red.offset >= var->blue.offset)
64 				return PIXFMT_RGB565;
65 			else
66 				return PIXFMT_BGR565;
67 		}
68 	}
69 
70 	/*
71 	 * Check for 888/A888.
72 	 */
73 	if (var->bits_per_pixel <= 32 && var->red.length <= 8 &&
74 			var->green.length <= 8 && var->blue.length <= 8) {
75 		if (var->bits_per_pixel == 24 && var->transp.length == 0) {
76 			if (var->red.offset >= var->blue.offset)
77 				return PIXFMT_RGB888PACK;
78 			else
79 				return PIXFMT_BGR888PACK;
80 		}
81 
82 		if (var->bits_per_pixel == 32 && var->transp.offset == 24) {
83 			if (var->red.offset >= var->blue.offset)
84 				return PIXFMT_RGBA888;
85 			else
86 				return PIXFMT_BGRA888;
87 		} else {
88 			if (var->red.offset >= var->blue.offset)
89 				return PIXFMT_RGB888UNPACK;
90 			else
91 				return PIXFMT_BGR888UNPACK;
92 		}
93 	}
94 
95 	return -EINVAL;
96 }
97 
98 static void pixfmt_to_var(struct fb_var_screeninfo *var, int pix_fmt)
99 {
100 	switch (pix_fmt) {
101 	case PIXFMT_RGB565:
102 		var->bits_per_pixel = 16;
103 		var->red.offset = 11;	var->red.length = 5;
104 		var->green.offset = 5;   var->green.length = 6;
105 		var->blue.offset = 0;	var->blue.length = 5;
106 		var->transp.offset = 0;  var->transp.length = 0;
107 		break;
108 	case PIXFMT_BGR565:
109 		var->bits_per_pixel = 16;
110 		var->red.offset = 0;	var->red.length = 5;
111 		var->green.offset = 5;	 var->green.length = 6;
112 		var->blue.offset = 11;	var->blue.length = 5;
113 		var->transp.offset = 0;  var->transp.length = 0;
114 		break;
115 	case PIXFMT_RGB888UNPACK:
116 		var->bits_per_pixel = 32;
117 		var->red.offset = 16;	var->red.length = 8;
118 		var->green.offset = 8;   var->green.length = 8;
119 		var->blue.offset = 0;	var->blue.length = 8;
120 		var->transp.offset = 0;  var->transp.length = 0;
121 		break;
122 	case PIXFMT_BGR888UNPACK:
123 		var->bits_per_pixel = 32;
124 		var->red.offset = 0;	var->red.length = 8;
125 		var->green.offset = 8;	 var->green.length = 8;
126 		var->blue.offset = 16;	var->blue.length = 8;
127 		var->transp.offset = 0;  var->transp.length = 0;
128 		break;
129 	case PIXFMT_RGBA888:
130 		var->bits_per_pixel = 32;
131 		var->red.offset = 16;	var->red.length = 8;
132 		var->green.offset = 8;   var->green.length = 8;
133 		var->blue.offset = 0;	var->blue.length = 8;
134 		var->transp.offset = 24; var->transp.length = 8;
135 		break;
136 	case PIXFMT_BGRA888:
137 		var->bits_per_pixel = 32;
138 		var->red.offset = 0;	var->red.length = 8;
139 		var->green.offset = 8;	 var->green.length = 8;
140 		var->blue.offset = 16;	var->blue.length = 8;
141 		var->transp.offset = 24; var->transp.length = 8;
142 		break;
143 	case PIXFMT_RGB888PACK:
144 		var->bits_per_pixel = 24;
145 		var->red.offset = 16;	var->red.length = 8;
146 		var->green.offset = 8;   var->green.length = 8;
147 		var->blue.offset = 0;	var->blue.length = 8;
148 		var->transp.offset = 0;  var->transp.length = 0;
149 		break;
150 	case PIXFMT_BGR888PACK:
151 		var->bits_per_pixel = 24;
152 		var->red.offset = 0;	var->red.length = 8;
153 		var->green.offset = 8;	 var->green.length = 8;
154 		var->blue.offset = 16;	var->blue.length = 8;
155 		var->transp.offset = 0;  var->transp.length = 0;
156 		break;
157 	case PIXFMT_YUV420P:
158 		var->bits_per_pixel = 12;
159 		var->red.offset = 4;	 var->red.length = 8;
160 		var->green.offset = 2;   var->green.length = 2;
161 		var->blue.offset = 0;   var->blue.length = 2;
162 		var->transp.offset = 0;  var->transp.length = 0;
163 		break;
164 	case PIXFMT_YVU420P:
165 		var->bits_per_pixel = 12;
166 		var->red.offset = 4;	 var->red.length = 8;
167 		var->green.offset = 0;	 var->green.length = 2;
168 		var->blue.offset = 2;	var->blue.length = 2;
169 		var->transp.offset = 0;  var->transp.length = 0;
170 		break;
171 	case PIXFMT_YUV422P:
172 		var->bits_per_pixel = 16;
173 		var->red.offset = 8;	 var->red.length = 8;
174 		var->green.offset = 4;   var->green.length = 4;
175 		var->blue.offset = 0;   var->blue.length = 4;
176 		var->transp.offset = 0;  var->transp.length = 0;
177 		break;
178 	case PIXFMT_YVU422P:
179 		var->bits_per_pixel = 16;
180 		var->red.offset = 8;	 var->red.length = 8;
181 		var->green.offset = 0;	 var->green.length = 4;
182 		var->blue.offset = 4;	var->blue.length = 4;
183 		var->transp.offset = 0;  var->transp.length = 0;
184 		break;
185 	case PIXFMT_UYVY:
186 		var->bits_per_pixel = 16;
187 		var->red.offset = 8;	 var->red.length = 16;
188 		var->green.offset = 4;   var->green.length = 16;
189 		var->blue.offset = 0;   var->blue.length = 16;
190 		var->transp.offset = 0;  var->transp.length = 0;
191 		break;
192 	case PIXFMT_VYUY:
193 		var->bits_per_pixel = 16;
194 		var->red.offset = 8;	 var->red.length = 16;
195 		var->green.offset = 0;	 var->green.length = 16;
196 		var->blue.offset = 4;	var->blue.length = 16;
197 		var->transp.offset = 0;  var->transp.length = 0;
198 		break;
199 	case PIXFMT_YUYV:
200 		var->bits_per_pixel = 16;
201 		var->red.offset = 0;	 var->red.length = 16;
202 		var->green.offset = 4;	 var->green.length = 16;
203 		var->blue.offset = 8;	var->blue.length = 16;
204 		var->transp.offset = 0;  var->transp.length = 0;
205 		break;
206 	case PIXFMT_PSEUDOCOLOR:
207 		var->bits_per_pixel = 8;
208 		var->red.offset = 0;	 var->red.length = 8;
209 		var->green.offset = 0;   var->green.length = 8;
210 		var->blue.offset = 0;	var->blue.length = 8;
211 		var->transp.offset = 0;  var->transp.length = 0;
212 		break;
213 	}
214 }
215 
216 /*
217  * fb framework has its limitation:
218  * 1. input color/output color is not seprated
219  * 2. fb_videomode not include output color
220  * so for fb usage, we keep a output format which is not changed
221  *  then it's added for mmpmode
222  */
223 static void fbmode_to_mmpmode(struct mmp_mode *mode,
224 		struct fb_videomode *videomode, int output_fmt)
225 {
226 	u64 div_result = 1000000000000ll;
227 	mode->name = videomode->name;
228 	mode->refresh = videomode->refresh;
229 	mode->xres = videomode->xres;
230 	mode->yres = videomode->yres;
231 
232 	do_div(div_result, videomode->pixclock);
233 	mode->pixclock_freq = (u32)div_result;
234 
235 	mode->left_margin = videomode->left_margin;
236 	mode->right_margin = videomode->right_margin;
237 	mode->upper_margin = videomode->upper_margin;
238 	mode->lower_margin = videomode->lower_margin;
239 	mode->hsync_len = videomode->hsync_len;
240 	mode->vsync_len = videomode->vsync_len;
241 	mode->hsync_invert = !!(videomode->sync & FB_SYNC_HOR_HIGH_ACT);
242 	mode->vsync_invert = !!(videomode->sync & FB_SYNC_VERT_HIGH_ACT);
243 	/* no defined flag in fb, use vmode>>3*/
244 	mode->invert_pixclock = !!(videomode->vmode & 8);
245 	mode->pix_fmt_out = output_fmt;
246 }
247 
248 static void mmpmode_to_fbmode(struct fb_videomode *videomode,
249 		struct mmp_mode *mode)
250 {
251 	u64 div_result = 1000000000000ll;
252 
253 	videomode->name = mode->name;
254 	videomode->refresh = mode->refresh;
255 	videomode->xres = mode->xres;
256 	videomode->yres = mode->yres;
257 
258 	do_div(div_result, mode->pixclock_freq);
259 	videomode->pixclock = (u32)div_result;
260 
261 	videomode->left_margin = mode->left_margin;
262 	videomode->right_margin = mode->right_margin;
263 	videomode->upper_margin = mode->upper_margin;
264 	videomode->lower_margin = mode->lower_margin;
265 	videomode->hsync_len = mode->hsync_len;
266 	videomode->vsync_len = mode->vsync_len;
267 	videomode->sync = (mode->hsync_invert ? FB_SYNC_HOR_HIGH_ACT : 0)
268 		| (mode->vsync_invert ? FB_SYNC_VERT_HIGH_ACT : 0);
269 	videomode->vmode = mode->invert_pixclock ? 8 : 0;
270 }
271 
272 static int mmpfb_check_var(struct fb_var_screeninfo *var,
273 		struct fb_info *info)
274 {
275 	struct mmpfb_info *fbi = info->par;
276 
277 	if (var->bits_per_pixel == 8)
278 		return -EINVAL;
279 	/*
280 	 * Basic geometry sanity checks.
281 	 */
282 	if (var->xoffset + var->xres > var->xres_virtual)
283 		return -EINVAL;
284 	if (var->yoffset + var->yres > var->yres_virtual)
285 		return -EINVAL;
286 
287 	/*
288 	 * Check size of framebuffer.
289 	 */
290 	if (var->xres_virtual * var->yres_virtual *
291 			(var->bits_per_pixel >> 3) > fbi->fb_size)
292 		return -EINVAL;
293 
294 	return 0;
295 }
296 
297 static unsigned int chan_to_field(unsigned int chan, struct fb_bitfield *bf)
298 {
299 	return ((chan & 0xffff) >> (16 - bf->length)) << bf->offset;
300 }
301 
302 static u32 to_rgb(u16 red, u16 green, u16 blue)
303 {
304 	red >>= 8;
305 	green >>= 8;
306 	blue >>= 8;
307 
308 	return (red << 16) | (green << 8) | blue;
309 }
310 
311 static int mmpfb_setcolreg(unsigned int regno, unsigned int red,
312 		unsigned int green, unsigned int blue,
313 		unsigned int trans, struct fb_info *info)
314 {
315 	struct mmpfb_info *fbi = info->par;
316 	u32 val;
317 
318 	if (info->fix.visual == FB_VISUAL_TRUECOLOR && regno < 16) {
319 		val =  chan_to_field(red,   &info->var.red);
320 		val |= chan_to_field(green, &info->var.green);
321 		val |= chan_to_field(blue , &info->var.blue);
322 		fbi->pseudo_palette[regno] = val;
323 	}
324 
325 	if (info->fix.visual == FB_VISUAL_PSEUDOCOLOR && regno < 256) {
326 		val = to_rgb(red, green, blue);
327 		/* TODO */
328 	}
329 
330 	return 0;
331 }
332 
333 static int mmpfb_pan_display(struct fb_var_screeninfo *var,
334 		struct fb_info *info)
335 {
336 	struct mmpfb_info *fbi = info->par;
337 	struct mmp_addr addr;
338 
339 	memset(&addr, 0, sizeof(addr));
340 	addr.phys[0] = (var->yoffset * var->xres_virtual + var->xoffset)
341 		* var->bits_per_pixel / 8 + fbi->fb_start_dma;
342 	mmp_overlay_set_addr(fbi->overlay, &addr);
343 
344 	return 0;
345 }
346 
347 static int var_update(struct fb_info *info)
348 {
349 	struct mmpfb_info *fbi = info->par;
350 	struct fb_var_screeninfo *var = &info->var;
351 	struct fb_videomode *m;
352 	int pix_fmt;
353 
354 	/* set pix_fmt */
355 	pix_fmt = var_to_pixfmt(var);
356 	if (pix_fmt < 0)
357 		return -EINVAL;
358 	pixfmt_to_var(var, pix_fmt);
359 	fbi->pix_fmt = pix_fmt;
360 
361 	/* set var according to best video mode*/
362 	m = (struct fb_videomode *)fb_match_mode(var, &info->modelist);
363 	if (!m) {
364 		dev_err(fbi->dev, "set par: no match mode, use best mode\n");
365 		m = (struct fb_videomode *)fb_find_best_mode(var,
366 				&info->modelist);
367 		fb_videomode_to_var(var, m);
368 	}
369 	memcpy(&fbi->mode, m, sizeof(struct fb_videomode));
370 
371 	/* fix to 2* yres */
372 	var->yres_virtual = var->yres * 2;
373 	info->fix.visual = (pix_fmt == PIXFMT_PSEUDOCOLOR) ?
374 		FB_VISUAL_PSEUDOCOLOR : FB_VISUAL_TRUECOLOR;
375 	info->fix.line_length = var->xres_virtual * var->bits_per_pixel / 8;
376 	info->fix.ypanstep = var->yres;
377 	return 0;
378 }
379 
380 static void mmpfb_set_win(struct fb_info *info)
381 {
382 	struct mmpfb_info *fbi = info->par;
383 	struct fb_var_screeninfo *var = &info->var;
384 	struct mmp_win win;
385 	u32 stride;
386 
387 	memset(&win, 0, sizeof(win));
388 	win.xsrc = win.xdst = fbi->mode.xres;
389 	win.ysrc = win.ydst = fbi->mode.yres;
390 	win.pix_fmt = fbi->pix_fmt;
391 	stride = pixfmt_to_stride(win.pix_fmt);
392 	win.pitch[0] = var->xres_virtual * stride;
393 	win.pitch[1] = win.pitch[2] =
394 		(stride == 1) ? (var->xres_virtual >> 1) : 0;
395 	mmp_overlay_set_win(fbi->overlay, &win);
396 }
397 
398 static int mmpfb_set_par(struct fb_info *info)
399 {
400 	struct mmpfb_info *fbi = info->par;
401 	struct fb_var_screeninfo *var = &info->var;
402 	struct mmp_addr addr;
403 	struct mmp_mode mode;
404 	int ret;
405 
406 	ret = var_update(info);
407 	if (ret != 0)
408 		return ret;
409 
410 	/* set window/path according to new videomode */
411 	fbmode_to_mmpmode(&mode, &fbi->mode, fbi->output_fmt);
412 	mmp_path_set_mode(fbi->path, &mode);
413 
414 	/* set window related info */
415 	mmpfb_set_win(info);
416 
417 	/* set address always */
418 	memset(&addr, 0, sizeof(addr));
419 	addr.phys[0] = (var->yoffset * var->xres_virtual + var->xoffset)
420 		* var->bits_per_pixel / 8 + fbi->fb_start_dma;
421 	mmp_overlay_set_addr(fbi->overlay, &addr);
422 
423 	return 0;
424 }
425 
426 static void mmpfb_power(struct mmpfb_info *fbi, int power)
427 {
428 	struct mmp_addr addr;
429 	struct fb_var_screeninfo *var = &fbi->fb_info->var;
430 
431 	/* for power on, always set address/window again */
432 	if (power) {
433 		/* set window related info */
434 		mmpfb_set_win(fbi->fb_info);
435 
436 		/* set address always */
437 		memset(&addr, 0, sizeof(addr));
438 		addr.phys[0] = fbi->fb_start_dma +
439 			(var->yoffset * var->xres_virtual + var->xoffset)
440 			* var->bits_per_pixel / 8;
441 		mmp_overlay_set_addr(fbi->overlay, &addr);
442 	}
443 	mmp_overlay_set_onoff(fbi->overlay, power);
444 }
445 
446 static int mmpfb_blank(int blank, struct fb_info *info)
447 {
448 	struct mmpfb_info *fbi = info->par;
449 
450 	mmpfb_power(fbi, (blank == FB_BLANK_UNBLANK));
451 
452 	return 0;
453 }
454 
455 static const struct fb_ops mmpfb_ops = {
456 	.owner		= THIS_MODULE,
457 	FB_DEFAULT_IOMEM_OPS,
458 	.fb_blank	= mmpfb_blank,
459 	.fb_check_var	= mmpfb_check_var,
460 	.fb_set_par	= mmpfb_set_par,
461 	.fb_setcolreg	= mmpfb_setcolreg,
462 	.fb_pan_display	= mmpfb_pan_display,
463 };
464 
465 static int modes_setup(struct mmpfb_info *fbi)
466 {
467 	struct fb_videomode *videomodes;
468 	struct mmp_mode *mmp_modes;
469 	struct fb_info *info = fbi->fb_info;
470 	int videomode_num, i;
471 
472 	/* get videomodes from path */
473 	videomode_num = mmp_path_get_modelist(fbi->path, &mmp_modes);
474 	if (!videomode_num) {
475 		dev_warn(fbi->dev, "can't get videomode num\n");
476 		return 0;
477 	}
478 	/* put videomode list to info structure */
479 	videomodes = kzalloc_objs(struct fb_videomode, videomode_num);
480 	if (!videomodes)
481 		return -ENOMEM;
482 
483 	for (i = 0; i < videomode_num; i++)
484 		mmpmode_to_fbmode(&videomodes[i], &mmp_modes[i]);
485 	fb_videomode_to_modelist(videomodes, videomode_num, &info->modelist);
486 
487 	/* set videomode[0] as default mode */
488 	memcpy(&fbi->mode, &videomodes[0], sizeof(struct fb_videomode));
489 	fbi->output_fmt = mmp_modes[0].pix_fmt_out;
490 	fb_videomode_to_var(&info->var, &fbi->mode);
491 	mmp_path_set_mode(fbi->path, &mmp_modes[0]);
492 
493 	kfree(videomodes);
494 	return videomode_num;
495 }
496 
497 static int fb_info_setup(struct fb_info *info,
498 			struct mmpfb_info *fbi)
499 {
500 	int ret = 0;
501 	/* Initialise static fb parameters.*/
502 	info->flags = FBINFO_PARTIAL_PAN_OK |
503 		FBINFO_HWACCEL_XPAN | FBINFO_HWACCEL_YPAN;
504 	info->node = -1;
505 	strcpy(info->fix.id, fbi->name);
506 	info->fix.type = FB_TYPE_PACKED_PIXELS;
507 	info->fix.type_aux = 0;
508 	info->fix.xpanstep = 0;
509 	info->fix.ypanstep = info->var.yres;
510 	info->fix.ywrapstep = 0;
511 	info->fix.accel = FB_ACCEL_NONE;
512 	info->fix.smem_start = fbi->fb_start_dma;
513 	info->fix.smem_len = fbi->fb_size;
514 	info->fix.visual = (fbi->pix_fmt == PIXFMT_PSEUDOCOLOR) ?
515 		FB_VISUAL_PSEUDOCOLOR : FB_VISUAL_TRUECOLOR;
516 	info->fix.line_length = info->var.xres_virtual *
517 		info->var.bits_per_pixel / 8;
518 	info->fbops = &mmpfb_ops;
519 	info->pseudo_palette = fbi->pseudo_palette;
520 	info->screen_buffer = fbi->fb_start;
521 	info->screen_size = fbi->fb_size;
522 
523 	/* For FB framework: Allocate color map and Register framebuffer*/
524 	if (fb_alloc_cmap(&info->cmap, 256, 0) < 0)
525 		ret = -ENOMEM;
526 
527 	return ret;
528 }
529 
530 static void fb_info_clear(struct fb_info *info)
531 {
532 	fb_dealloc_cmap(&info->cmap);
533 }
534 
535 static int mmpfb_probe(struct platform_device *pdev)
536 {
537 	struct mmp_buffer_driver_mach_info *mi;
538 	struct fb_info *info;
539 	struct mmpfb_info *fbi;
540 	int ret, modes_num;
541 
542 	mi = pdev->dev.platform_data;
543 	if (mi == NULL) {
544 		dev_err(&pdev->dev, "no platform data defined\n");
545 		return -EINVAL;
546 	}
547 
548 	/* initialize fb */
549 	info = framebuffer_alloc(sizeof(struct mmpfb_info), &pdev->dev);
550 	if (info == NULL)
551 		return -ENOMEM;
552 	fbi = info->par;
553 
554 	/* init fb */
555 	fbi->fb_info = info;
556 	platform_set_drvdata(pdev, fbi);
557 	fbi->dev = &pdev->dev;
558 	fbi->name = mi->name;
559 	fbi->pix_fmt = mi->default_pixfmt;
560 	pixfmt_to_var(&info->var, fbi->pix_fmt);
561 	mutex_init(&fbi->access_ok);
562 
563 	/* get display path by name */
564 	fbi->path = mmp_get_path(mi->path_name);
565 	if (!fbi->path) {
566 		dev_err(&pdev->dev, "can't get the path %s\n", mi->path_name);
567 		ret = -EINVAL;
568 		goto failed_destroy_mutex;
569 	}
570 
571 	dev_info(fbi->dev, "path %s get\n", fbi->path->name);
572 
573 	/* get overlay */
574 	fbi->overlay = mmp_path_get_overlay(fbi->path, mi->overlay_id);
575 	if (!fbi->overlay) {
576 		ret = -EINVAL;
577 		goto failed_destroy_mutex;
578 	}
579 	/* set fetch used */
580 	mmp_overlay_set_fetch(fbi->overlay, mi->dmafetch_id);
581 
582 	modes_num = modes_setup(fbi);
583 	if (modes_num < 0) {
584 		ret = modes_num;
585 		goto failed_destroy_mutex;
586 	}
587 
588 	/*
589 	 * if get modes success, means not hotplug panels, use caculated buffer
590 	 * or use default size
591 	 */
592 	if (modes_num > 0) {
593 		/* fix to 2* yres */
594 		info->var.yres_virtual = info->var.yres * 2;
595 
596 		/* Allocate framebuffer memory: size = modes xy *4 */
597 		fbi->fb_size = info->var.xres_virtual * info->var.yres_virtual
598 				* info->var.bits_per_pixel / 8;
599 	} else {
600 		fbi->fb_size = MMPFB_DEFAULT_SIZE;
601 	}
602 
603 	fbi->fb_start = dma_alloc_coherent(&pdev->dev, PAGE_ALIGN(fbi->fb_size),
604 				&fbi->fb_start_dma, GFP_KERNEL);
605 	if (fbi->fb_start == NULL) {
606 		dev_err(&pdev->dev, "can't alloc framebuffer\n");
607 		ret = -ENOMEM;
608 		goto failed_destroy_mutex;
609 	}
610 	dev_info(fbi->dev, "fb %dk allocated\n", fbi->fb_size/1024);
611 
612 	/* fb power on */
613 	if (modes_num > 0)
614 		mmpfb_power(fbi, 1);
615 
616 	ret = fb_info_setup(info, fbi);
617 	if (ret < 0)
618 		goto failed_free_buff;
619 
620 	ret = register_framebuffer(info);
621 	if (ret < 0) {
622 		dev_err(&pdev->dev, "Failed to register fb: %d\n", ret);
623 		ret = -ENXIO;
624 		goto failed_clear_info;
625 	}
626 
627 	dev_info(fbi->dev, "loaded to /dev/fb%d <%s>.\n",
628 		info->node, info->fix.id);
629 
630 	return 0;
631 
632 failed_clear_info:
633 	fb_info_clear(info);
634 failed_free_buff:
635 	dma_free_coherent(&pdev->dev, PAGE_ALIGN(fbi->fb_size), fbi->fb_start,
636 		fbi->fb_start_dma);
637 failed_destroy_mutex:
638 	mutex_destroy(&fbi->access_ok);
639 	dev_err(fbi->dev, "mmp-fb: frame buffer device init failed\n");
640 
641 	framebuffer_release(info);
642 
643 	return ret;
644 }
645 
646 static struct platform_driver mmpfb_driver = {
647 	.driver		= {
648 		.name	= "mmp-fb",
649 	},
650 	.probe		= mmpfb_probe,
651 };
652 
653 static int mmpfb_init(void)
654 {
655 	return platform_driver_register(&mmpfb_driver);
656 }
657 module_init(mmpfb_init);
658 
659 MODULE_AUTHOR("Zhou Zhu <zhou.zhu@marvell.com>");
660 MODULE_DESCRIPTION("Framebuffer driver for Marvell displays");
661 MODULE_LICENSE("GPL");
662