drivers/video/fbdev/ocfb.c - linux - Git at Google

 /*
  * OpenCores VGA/LCD 2.0 core frame buffer driver
  *
  * Copyright (C) 2013 Stefan Kristiansson, stefan.kristiansson@saunalahti.fi
  *
  * This file is licensed under the terms of the GNU General Public License
  * version 2.  This program is licensed "as is" without any warranty of any
  * kind, whether express or implied.
  */

 #include <linux/delay.h>
 #include <linux/dma-mapping.h>
 #include <linux/errno.h>
 #include <linux/fb.h>
 #include <linux/init.h>
 #include <linux/io.h>
 #include <linux/kernel.h>
 #include <linux/mm.h>
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/string.h>
 #include <linux/slab.h>

 /* OCFB register defines */
 #define OCFB_CTRL	0x000
 #define OCFB_STAT	0x004
 #define OCFB_HTIM	0x008
 #define OCFB_VTIM	0x00c
 #define OCFB_HVLEN	0x010
 #define OCFB_VBARA	0x014
 #define OCFB_PALETTE	0x800

 #define OCFB_CTRL_VEN	0x00000001 /* Video Enable */
 #define OCFB_CTRL_HIE	0x00000002 /* HSync Interrupt Enable */
 #define OCFB_CTRL_PC	0x00000800 /* 8-bit Pseudo Color Enable*/
 #define OCFB_CTRL_CD8	0x00000000 /* Color Depth 8 */
 #define OCFB_CTRL_CD16	0x00000200 /* Color Depth 16 */
 #define OCFB_CTRL_CD24	0x00000400 /* Color Depth 24 */
 #define OCFB_CTRL_CD32	0x00000600 /* Color Depth 32 */
 #define OCFB_CTRL_VBL1	0x00000000 /* Burst Length 1 */
 #define OCFB_CTRL_VBL2	0x00000080 /* Burst Length 2 */
 #define OCFB_CTRL_VBL4	0x00000100 /* Burst Length 4 */
 #define OCFB_CTRL_VBL8	0x00000180 /* Burst Length 8 */

 #define PALETTE_SIZE	256

 #define OCFB_NAME	"OC VGA/LCD"

 static char *mode_option;

 static const struct fb_videomode default_mode = {
 	/* 640x480 @ 60 Hz, 31.5 kHz hsync */
 	NULL, 60, 640, 480, 39721, 40, 24, 32, 11, 96, 2,
 	0, FB_VMODE_NONINTERLACED
 };

 struct ocfb_dev {
 	struct fb_info info;
 	void __iomem *regs;
 	/* flag indicating whether the regs are little endian accessed */
 	int little_endian;
 	/* Physical and virtual addresses of framebuffer */
 	dma_addr_t fb_phys;
 	void __iomem *fb_virt;
 	u32 pseudo_palette[PALETTE_SIZE];
 };

 #ifndef MODULE
 static int __init ocfb_setup(char *options)
 {
 	char *curr_opt;

 	if (!options || !*options)
 		return 0;

 	while ((curr_opt = strsep(&options, ",")) != NULL) {
 		if (!*curr_opt)
 			continue;
 		mode_option = curr_opt;
 	}

 	return 0;
 }
 #endif

 static inline u32 ocfb_readreg(struct ocfb_dev *fbdev, loff_t offset)
 {
 	if (fbdev->little_endian)
 		return ioread32(fbdev->regs + offset);
 	else
 		return ioread32be(fbdev->regs + offset);
 }

 static void ocfb_writereg(struct ocfb_dev *fbdev, loff_t offset, u32 data)
 {
 	if (fbdev->little_endian)
 		iowrite32(data, fbdev->regs + offset);
 	else
 		iowrite32be(data, fbdev->regs + offset);
 }

 static int ocfb_setupfb(struct ocfb_dev *fbdev)
 {
 	unsigned long bpp_config;
 	struct fb_var_screeninfo *var = &fbdev->info.var;
 	struct device *dev = fbdev->info.device;
 	u32 hlen;
 	u32 vlen;

 	/* Disable display */
 	ocfb_writereg(fbdev, OCFB_CTRL, 0);

 	/* Register framebuffer address */
 	fbdev->little_endian = 0;
 	ocfb_writereg(fbdev, OCFB_VBARA, fbdev->fb_phys);

 	/* Detect endianess */
 	if (ocfb_readreg(fbdev, OCFB_VBARA) != fbdev->fb_phys) {
 		fbdev->little_endian = 1;
 		ocfb_writereg(fbdev, OCFB_VBARA, fbdev->fb_phys);
 	}

 	/* Horizontal timings */
 	ocfb_writereg(fbdev, OCFB_HTIM, (var->hsync_len - 1) << 24 |
 		      (var->left_margin - 1) << 16 | (var->xres - 1));

 	/* Vertical timings */
 	ocfb_writereg(fbdev, OCFB_VTIM, (var->vsync_len - 1) << 24 |
 		      (var->upper_margin - 1) << 16 | (var->yres - 1));

 	/* Total length of frame */
 	hlen = var->left_margin + var->right_margin + var->hsync_len +
 		var->xres;

 	vlen = var->upper_margin + var->lower_margin + var->vsync_len +
 		var->yres;

 	ocfb_writereg(fbdev, OCFB_HVLEN, (hlen - 1) << 16 | (vlen - 1));

 	bpp_config = OCFB_CTRL_CD8;
 	switch (var->bits_per_pixel) {
 	case 8:
 		if (!var->grayscale)
 			bpp_config |= OCFB_CTRL_PC;  /* enable palette */
 		break;

 	case 16:
 		bpp_config |= OCFB_CTRL_CD16;
 		break;

 	case 24:
 		bpp_config |= OCFB_CTRL_CD24;
 		break;

 	case 32:
 		bpp_config |= OCFB_CTRL_CD32;
 		break;

 	default:
 		dev_err(dev, "no bpp specified\n");
 		break;
 	}

 	/* maximum (8) VBL (video memory burst length) */
 	bpp_config |= OCFB_CTRL_VBL8;

 	/* Enable output */
 	ocfb_writereg(fbdev, OCFB_CTRL, (OCFB_CTRL_VEN | bpp_config));

 	return 0;
 }

 static int ocfb_setcolreg(unsigned regno, unsigned red, unsigned green,
 			  unsigned blue, unsigned transp,
 			  struct fb_info *info)
 {
 	struct ocfb_dev *fbdev = (struct ocfb_dev *)info->par;
 	u32 color;

 	if (regno >= info->cmap.len) {
 		dev_err(info->device, "regno >= cmap.len\n");
 		return 1;
 	}

 	if (info->var.grayscale) {
 		/* grayscale = 0.30*R + 0.59*G + 0.11*B */
 		red = green = blue = (red * 77 + green * 151 + blue * 28) >> 8;
 	}

 	red >>= (16 - info->var.red.length);
 	green >>= (16 - info->var.green.length);
 	blue >>= (16 - info->var.blue.length);
 	transp >>= (16 - info->var.transp.length);

 	if (info->var.bits_per_pixel == 8 && !info->var.grayscale) {
 		regno <<= 2;
 		color = (red << 16) | (green << 8) | blue;
 		ocfb_writereg(fbdev, OCFB_PALETTE + regno, color);
 	} else {
 		((u32 *)(info->pseudo_palette))[regno] =
 			(red << info->var.red.offset) |
 			(green << info->var.green.offset) |
 			(blue << info->var.blue.offset) |
 			(transp << info->var.transp.offset);
 	}

 	return 0;
 }

 static int ocfb_init_fix(struct ocfb_dev *fbdev)
 {
 	struct fb_var_screeninfo *var = &fbdev->info.var;
 	struct fb_fix_screeninfo *fix = &fbdev->info.fix;

 	strcpy(fix->id, OCFB_NAME);

 	fix->line_length = var->xres * var->bits_per_pixel/8;
 	fix->smem_len = fix->line_length * var->yres;
 	fix->type = FB_TYPE_PACKED_PIXELS;

 	if (var->bits_per_pixel == 8 && !var->grayscale)
 		fix->visual = FB_VISUAL_PSEUDOCOLOR;
 	else
 		fix->visual = FB_VISUAL_TRUECOLOR;

 	return 0;
 }

 static int ocfb_init_var(struct ocfb_dev *fbdev)
 {
 	struct fb_var_screeninfo *var = &fbdev->info.var;

 	var->accel_flags = FB_ACCEL_NONE;
 	var->activate = FB_ACTIVATE_NOW;
 	var->xres_virtual = var->xres;
 	var->yres_virtual = var->yres;

 	switch (var->bits_per_pixel) {
 	case 8:
 		var->transp.offset = 0;
 		var->transp.length = 0;
 		var->red.offset = 0;
 		var->red.length = 8;
 		var->green.offset = 0;
 		var->green.length = 8;
 		var->blue.offset = 0;
 		var->blue.length = 8;
 		break;

 	case 16:
 		var->transp.offset = 0;
 		var->transp.length = 0;
 		var->red.offset = 11;
 		var->red.length = 5;
 		var->green.offset = 5;
 		var->green.length = 6;
 		var->blue.offset = 0;
 		var->blue.length  = 5;
 		break;

 	case 24:
 		var->transp.offset = 0;
 		var->transp.length = 0;
 		var->red.offset = 16;
 		var->red.length = 8;
 		var->green.offset = 8;
 		var->green.length = 8;
 		var->blue.offset = 0;
 		var->blue.length = 8;
 		break;

 	case 32:
 		var->transp.offset = 24;
 		var->transp.length = 8;
 		var->red.offset = 16;
 		var->red.length = 8;
 		var->green.offset = 8;
 		var->green.length = 8;
 		var->blue.offset = 0;
 		var->blue.length = 8;
 		break;
 	}

 	return 0;
 }

 static const struct fb_ops ocfb_ops = {
 	.owner		= THIS_MODULE,
 	FB_DEFAULT_IOMEM_OPS,
 	.fb_setcolreg	= ocfb_setcolreg,
 };

 static int ocfb_probe(struct platform_device *pdev)
 {
 	int ret = 0;
 	struct ocfb_dev *fbdev;
 	int fbsize;

 	fbdev = devm_kzalloc(&pdev->dev, sizeof(*fbdev), GFP_KERNEL);
 	if (!fbdev)
 		return -ENOMEM;

 	platform_set_drvdata(pdev, fbdev);

 	fbdev->info.fbops = &ocfb_ops;
 	fbdev->info.device = &pdev->dev;
 	fbdev->info.par = fbdev;

 	/* Video mode setup */
 	if (!fb_find_mode(&fbdev->info.var, &fbdev->info, mode_option,
 			  NULL, 0, &default_mode, 16)) {
 		dev_err(&pdev->dev, "No valid video modes found\n");
 		return -EINVAL;
 	}
 	ocfb_init_var(fbdev);
 	ocfb_init_fix(fbdev);

 	fbdev->regs = devm_platform_ioremap_resource(pdev, 0);
 	if (IS_ERR(fbdev->regs))
 		return PTR_ERR(fbdev->regs);

 	/* Allocate framebuffer memory */
 	fbsize = fbdev->info.fix.smem_len;
 	fbdev->fb_virt = dma_alloc_coherent(&pdev->dev, PAGE_ALIGN(fbsize),
 					    &fbdev->fb_phys, GFP_KERNEL);
 	if (!fbdev->fb_virt) {
 		dev_err(&pdev->dev,
 			"Frame buffer memory allocation failed\n");
 		return -ENOMEM;
 	}
 	fbdev->info.fix.smem_start = fbdev->fb_phys;
 	fbdev->info.screen_base = fbdev->fb_virt;
 	fbdev->info.pseudo_palette = fbdev->pseudo_palette;

 	/* Clear framebuffer */
 	memset_io(fbdev->fb_virt, 0, fbsize);

 	/* Setup and enable the framebuffer */
 	ocfb_setupfb(fbdev);

 	if (fbdev->little_endian)
 		fbdev->info.flags |= FBINFO_FOREIGN_ENDIAN;

 	/* Allocate color map */
 	ret = fb_alloc_cmap(&fbdev->info.cmap, PALETTE_SIZE, 0);
 	if (ret) {
 		dev_err(&pdev->dev, "Color map allocation failed\n");
 		goto err_dma_free;
 	}

 	/* Register framebuffer */
 	ret = register_framebuffer(&fbdev->info);
 	if (ret) {
 		dev_err(&pdev->dev, "Framebuffer registration failed\n");
 		goto err_dealloc_cmap;
 	}

 	return 0;

 err_dealloc_cmap:
 	fb_dealloc_cmap(&fbdev->info.cmap);

 err_dma_free:
 	dma_free_coherent(&pdev->dev, PAGE_ALIGN(fbsize), fbdev->fb_virt,
 			  fbdev->fb_phys);

 	return ret;
 }

 static void ocfb_remove(struct platform_device *pdev)
 {
 	struct ocfb_dev *fbdev = platform_get_drvdata(pdev);

 	unregister_framebuffer(&fbdev->info);
 	fb_dealloc_cmap(&fbdev->info.cmap);
 	dma_free_coherent(&pdev->dev, PAGE_ALIGN(fbdev->info.fix.smem_len),
 			  fbdev->fb_virt, fbdev->fb_phys);

 	/* Disable display */
 	ocfb_writereg(fbdev, OCFB_CTRL, 0);

 	platform_set_drvdata(pdev, NULL);
 }

 static const struct of_device_id ocfb_match[] = {
 	{ .compatible = "opencores,ocfb", },
 	{},
 };
 MODULE_DEVICE_TABLE(of, ocfb_match);

 static struct platform_driver ocfb_driver = {
 	.probe  = ocfb_probe,
 	.remove_new = ocfb_remove,
 	.driver = {
 		.name = "ocfb_fb",
 		.of_match_table = ocfb_match,
 	}
 };

 /*
  * Init and exit routines
  */
 static int __init ocfb_init(void)
 {
 #ifndef MODULE
 	char *option = NULL;

 	if (fb_get_options("ocfb", &option))
 		return -ENODEV;
 	ocfb_setup(option);
 #endif
 	return platform_driver_register(&ocfb_driver);
 }

 static void __exit ocfb_exit(void)
 {
 	platform_driver_unregister(&ocfb_driver);
 }

 module_init(ocfb_init);
 module_exit(ocfb_exit);

 MODULE_AUTHOR("Stefan Kristiansson <stefan.kristiansson@saunalahti.fi>");
 MODULE_DESCRIPTION("OpenCores VGA/LCD 2.0 frame buffer driver");
 MODULE_LICENSE("GPL v2");
 module_param(mode_option, charp, 0);
 MODULE_PARM_DESC(mode_option, "Video mode ('<xres>x<yres>[-<bpp>][@refresh]')");
	/*
	* OpenCores VGA/LCD 2.0 core frame buffer driver
	*
	* Copyright (C) 2013 Stefan Kristiansson, stefan.kristiansson@saunalahti.fi
	*
	* This file is licensed under the terms of the GNU General Public License
	* version 2. This program is licensed "as is" without any warranty of any
	* kind, whether express or implied.
	*/

	#include <linux/delay.h>
	#include <linux/dma-mapping.h>
	#include <linux/errno.h>
	#include <linux/fb.h>
	#include <linux/init.h>
	#include <linux/io.h>
	#include <linux/kernel.h>
	#include <linux/mm.h>
	#include <linux/module.h>
	#include <linux/of.h>
	#include <linux/platform_device.h>
	#include <linux/string.h>
	#include <linux/slab.h>

	/* OCFB register defines */
	#define OCFB_CTRL 0x000
	#define OCFB_STAT 0x004
	#define OCFB_HTIM 0x008
	#define OCFB_VTIM 0x00c
	#define OCFB_HVLEN 0x010
	#define OCFB_VBARA 0x014
	#define OCFB_PALETTE 0x800

	#define OCFB_CTRL_VEN 0x00000001 /* Video Enable */
	#define OCFB_CTRL_HIE 0x00000002 /* HSync Interrupt Enable */
	#define OCFB_CTRL_PC 0x00000800 /* 8-bit Pseudo Color Enable*/
	#define OCFB_CTRL_CD8 0x00000000 /* Color Depth 8 */
	#define OCFB_CTRL_CD16 0x00000200 /* Color Depth 16 */
	#define OCFB_CTRL_CD24 0x00000400 /* Color Depth 24 */
	#define OCFB_CTRL_CD32 0x00000600 /* Color Depth 32 */
	#define OCFB_CTRL_VBL1 0x00000000 /* Burst Length 1 */
	#define OCFB_CTRL_VBL2 0x00000080 /* Burst Length 2 */
	#define OCFB_CTRL_VBL4 0x00000100 /* Burst Length 4 */
	#define OCFB_CTRL_VBL8 0x00000180 /* Burst Length 8 */

	#define PALETTE_SIZE 256

	#define OCFB_NAME "OC VGA/LCD"

	static char *mode_option;

	static const struct fb_videomode default_mode = {
	/* 640x480 @ 60 Hz, 31.5 kHz hsync */
	NULL, 60, 640, 480, 39721, 40, 24, 32, 11, 96, 2,
	0, FB_VMODE_NONINTERLACED
	};

	struct ocfb_dev {
	struct fb_info info;
	void __iomem *regs;
	/* flag indicating whether the regs are little endian accessed */
	int little_endian;
	/* Physical and virtual addresses of framebuffer */
	dma_addr_t fb_phys;
	void __iomem *fb_virt;
	u32 pseudo_palette[PALETTE_SIZE];
	};

	#ifndef MODULE
	static int __init ocfb_setup(char *options)
	{
	char *curr_opt;

	if (!options \|\| !*options)
	return 0;

	while ((curr_opt = strsep(&options, ",")) != NULL) {
	if (!*curr_opt)
	continue;
	mode_option = curr_opt;
	}

	return 0;
	}
	#endif

	static inline u32 ocfb_readreg(struct ocfb_dev *fbdev, loff_t offset)
	{
	if (fbdev->little_endian)
	return ioread32(fbdev->regs + offset);
	else
	return ioread32be(fbdev->regs + offset);
	}

	static void ocfb_writereg(struct ocfb_dev *fbdev, loff_t offset, u32 data)
	{
	if (fbdev->little_endian)
	iowrite32(data, fbdev->regs + offset);
	else
	iowrite32be(data, fbdev->regs + offset);
	}

	static int ocfb_setupfb(struct ocfb_dev *fbdev)
	{
	unsigned long bpp_config;
	struct fb_var_screeninfo *var = &fbdev->info.var;
	struct device *dev = fbdev->info.device;
	u32 hlen;
	u32 vlen;

	/* Disable display */
	ocfb_writereg(fbdev, OCFB_CTRL, 0);

	/* Register framebuffer address */
	fbdev->little_endian = 0;
	ocfb_writereg(fbdev, OCFB_VBARA, fbdev->fb_phys);

	/* Detect endianess */
	if (ocfb_readreg(fbdev, OCFB_VBARA) != fbdev->fb_phys) {
	fbdev->little_endian = 1;
	ocfb_writereg(fbdev, OCFB_VBARA, fbdev->fb_phys);
	}

	/* Horizontal timings */
	ocfb_writereg(fbdev, OCFB_HTIM, (var->hsync_len - 1) << 24 \|
	(var->left_margin - 1) << 16 \| (var->xres - 1));

	/* Vertical timings */
	ocfb_writereg(fbdev, OCFB_VTIM, (var->vsync_len - 1) << 24 \|
	(var->upper_margin - 1) << 16 \| (var->yres - 1));

	/* Total length of frame */
	hlen = var->left_margin + var->right_margin + var->hsync_len +
	var->xres;

	vlen = var->upper_margin + var->lower_margin + var->vsync_len +
	var->yres;

	ocfb_writereg(fbdev, OCFB_HVLEN, (hlen - 1) << 16 \| (vlen - 1));

	bpp_config = OCFB_CTRL_CD8;
	switch (var->bits_per_pixel) {
	case 8:
	if (!var->grayscale)
	bpp_config \|= OCFB_CTRL_PC; /* enable palette */
	break;

	case 16:
	bpp_config \|= OCFB_CTRL_CD16;
	break;

	case 24:
	bpp_config \|= OCFB_CTRL_CD24;
	break;

	case 32:
	bpp_config \|= OCFB_CTRL_CD32;
	break;

	default:
	dev_err(dev, "no bpp specified\n");
	break;
	}

	/* maximum (8) VBL (video memory burst length) */
	bpp_config \|= OCFB_CTRL_VBL8;

	/* Enable output */
	ocfb_writereg(fbdev, OCFB_CTRL, (OCFB_CTRL_VEN \| bpp_config));

	return 0;
	}

	static int ocfb_setcolreg(unsigned regno, unsigned red, unsigned green,
	unsigned blue, unsigned transp,
	struct fb_info *info)
	{
	struct ocfb_dev fbdev = (struct ocfb_dev )info->par;
	u32 color;

	if (regno >= info->cmap.len) {
	dev_err(info->device, "regno >= cmap.len\n");
	return 1;
	}

	if (info->var.grayscale) {
	/* grayscale = 0.30R + 0.59G + 0.11B /
	red = green = blue = (red * 77 + green * 151 + blue * 28) >> 8;
	}

	red >>= (16 - info->var.red.length);
	green >>= (16 - info->var.green.length);
	blue >>= (16 - info->var.blue.length);
	transp >>= (16 - info->var.transp.length);

	if (info->var.bits_per_pixel == 8 && !info->var.grayscale) {
	regno <<= 2;
	color = (red << 16) \| (green << 8) \| blue;
	ocfb_writereg(fbdev, OCFB_PALETTE + regno, color);
	} else {
	((u32 *)(info->pseudo_palette))[regno] =
	(red << info->var.red.offset) \|
	(green << info->var.green.offset) \|
	(blue << info->var.blue.offset) \|
	(transp << info->var.transp.offset);
	}

	return 0;
	}

	static int ocfb_init_fix(struct ocfb_dev *fbdev)
	{
	struct fb_var_screeninfo *var = &fbdev->info.var;
	struct fb_fix_screeninfo *fix = &fbdev->info.fix;

	strcpy(fix->id, OCFB_NAME);

	fix->line_length = var->xres * var->bits_per_pixel/8;
	fix->smem_len = fix->line_length * var->yres;
	fix->type = FB_TYPE_PACKED_PIXELS;

	if (var->bits_per_pixel == 8 && !var->grayscale)
	fix->visual = FB_VISUAL_PSEUDOCOLOR;
	else
	fix->visual = FB_VISUAL_TRUECOLOR;

	return 0;
	}

	static int ocfb_init_var(struct ocfb_dev *fbdev)
	{
	struct fb_var_screeninfo *var = &fbdev->info.var;

	var->accel_flags = FB_ACCEL_NONE;
	var->activate = FB_ACTIVATE_NOW;
	var->xres_virtual = var->xres;
	var->yres_virtual = var->yres;

	switch (var->bits_per_pixel) {
	case 8:
	var->transp.offset = 0;
	var->transp.length = 0;
	var->red.offset = 0;
	var->red.length = 8;
	var->green.offset = 0;
	var->green.length = 8;
	var->blue.offset = 0;
	var->blue.length = 8;
	break;

	case 16:
	var->transp.offset = 0;
	var->transp.length = 0;
	var->red.offset = 11;
	var->red.length = 5;
	var->green.offset = 5;
	var->green.length = 6;
	var->blue.offset = 0;
	var->blue.length = 5;
	break;

	case 24:
	var->transp.offset = 0;
	var->transp.length = 0;
	var->red.offset = 16;
	var->red.length = 8;
	var->green.offset = 8;
	var->green.length = 8;
	var->blue.offset = 0;
	var->blue.length = 8;
	break;

	case 32:
	var->transp.offset = 24;
	var->transp.length = 8;
	var->red.offset = 16;
	var->red.length = 8;
	var->green.offset = 8;
	var->green.length = 8;
	var->blue.offset = 0;
	var->blue.length = 8;
	break;
	}

	return 0;
	}

	static const struct fb_ops ocfb_ops = {
	.owner = THIS_MODULE,
	FB_DEFAULT_IOMEM_OPS,
	.fb_setcolreg = ocfb_setcolreg,
	};

	static int ocfb_probe(struct platform_device *pdev)
	{
	int ret = 0;
	struct ocfb_dev *fbdev;
	int fbsize;

	fbdev = devm_kzalloc(&pdev->dev, sizeof(*fbdev), GFP_KERNEL);
	if (!fbdev)
	return -ENOMEM;

	platform_set_drvdata(pdev, fbdev);

	fbdev->info.fbops = &ocfb_ops;
	fbdev->info.device = &pdev->dev;
	fbdev->info.par = fbdev;

	/* Video mode setup */
	if (!fb_find_mode(&fbdev->info.var, &fbdev->info, mode_option,
	NULL, 0, &default_mode, 16)) {
	dev_err(&pdev->dev, "No valid video modes found\n");
	return -EINVAL;
	}
	ocfb_init_var(fbdev);
	ocfb_init_fix(fbdev);

	fbdev->regs = devm_platform_ioremap_resource(pdev, 0);
	if (IS_ERR(fbdev->regs))
	return PTR_ERR(fbdev->regs);

	/* Allocate framebuffer memory */
	fbsize = fbdev->info.fix.smem_len;
	fbdev->fb_virt = dma_alloc_coherent(&pdev->dev, PAGE_ALIGN(fbsize),
	&fbdev->fb_phys, GFP_KERNEL);
	if (!fbdev->fb_virt) {
	dev_err(&pdev->dev,
	"Frame buffer memory allocation failed\n");
	return -ENOMEM;
	}
	fbdev->info.fix.smem_start = fbdev->fb_phys;
	fbdev->info.screen_base = fbdev->fb_virt;
	fbdev->info.pseudo_palette = fbdev->pseudo_palette;

	/* Clear framebuffer */
	memset_io(fbdev->fb_virt, 0, fbsize);

	/* Setup and enable the framebuffer */
	ocfb_setupfb(fbdev);

	if (fbdev->little_endian)
	fbdev->info.flags \|= FBINFO_FOREIGN_ENDIAN;

	/* Allocate color map */
	ret = fb_alloc_cmap(&fbdev->info.cmap, PALETTE_SIZE, 0);
	if (ret) {
	dev_err(&pdev->dev, "Color map allocation failed\n");
	goto err_dma_free;
	}

	/* Register framebuffer */
	ret = register_framebuffer(&fbdev->info);
	if (ret) {
	dev_err(&pdev->dev, "Framebuffer registration failed\n");
	goto err_dealloc_cmap;
	}

	return 0;

	err_dealloc_cmap:
	fb_dealloc_cmap(&fbdev->info.cmap);

	err_dma_free:
	dma_free_coherent(&pdev->dev, PAGE_ALIGN(fbsize), fbdev->fb_virt,
	fbdev->fb_phys);

	return ret;
	}

	static void ocfb_remove(struct platform_device *pdev)
	{
	struct ocfb_dev *fbdev = platform_get_drvdata(pdev);

	unregister_framebuffer(&fbdev->info);
	fb_dealloc_cmap(&fbdev->info.cmap);
	dma_free_coherent(&pdev->dev, PAGE_ALIGN(fbdev->info.fix.smem_len),
	fbdev->fb_virt, fbdev->fb_phys);

	/* Disable display */
	ocfb_writereg(fbdev, OCFB_CTRL, 0);

	platform_set_drvdata(pdev, NULL);
	}

	static const struct of_device_id ocfb_match[] = {
	{ .compatible = "opencores,ocfb", },
	{},
	};
	MODULE_DEVICE_TABLE(of, ocfb_match);

	static struct platform_driver ocfb_driver = {
	.probe = ocfb_probe,
	.remove_new = ocfb_remove,
	.driver = {
	.name = "ocfb_fb",
	.of_match_table = ocfb_match,
	}
	};

	/*
	* Init and exit routines
	*/
	static int __init ocfb_init(void)
	{
	#ifndef MODULE
	char *option = NULL;

	if (fb_get_options("ocfb", &option))
	return -ENODEV;
	ocfb_setup(option);
	#endif
	return platform_driver_register(&ocfb_driver);
	}

	static void __exit ocfb_exit(void)
	{
	platform_driver_unregister(&ocfb_driver);
	}

	module_init(ocfb_init);
	module_exit(ocfb_exit);

	MODULE_AUTHOR("Stefan Kristiansson <stefan.kristiansson@saunalahti.fi>");
	MODULE_DESCRIPTION("OpenCores VGA/LCD 2.0 frame buffer driver");
	MODULE_LICENSE("GPL v2");
	module_param(mode_option, charp, 0);
	MODULE_PARM_DESC(mode_option, "Video mode ('<xres>x<yres>[-<bpp>][@refresh]')");