simplefb: add clock handling code

#include <linux/module.h>

#include <linux/platform_data/simplefb.h>

#include <linux/platform_device.h>

#include <linux/clk-provider.h>

static struct fb_fix_screeninfo simplefb_fix = {

	.id		= "simple",

struct simplefb_par {

	u32 palette[PSEUDO_PALETTE_SIZE];

#ifdef CONFIG_OF

	int clk_count;

	struct clk **clks;

#endif

};

#ifdef CONFIG_OF

/*

 * Clock handling code.

 *

 * Here we handle the clocks property of our "simple-framebuffer" dt node.

 * This is necessary so that we can make sure that any clocks needed by

 * the display engine that the bootloader set up for us (and for which it

 * provided a simplefb dt node), stay up, for the life of the simplefb

 * driver.

 *

 * When the driver unloads, we cleanly disable, and then release the clocks.

 *

 * We only complain about errors here, no action is taken as the most likely

 * error can only happen due to a mismatch between the bootloader which set

 * up simplefb, and the clock definitions in the device tree. Chances are

 * that there are no adverse effects, and if there are, a clean teardown of

 * the fb probe will not help us much either. So just complain and carry on,

 * and hope that the user actually gets a working fb at the end of things.

 */

static int simplefb_clocks_init(struct simplefb_par *par,

				struct platform_device *pdev)

{

	struct device_node *np = pdev->dev.of_node;

	struct clk *clock;

	int i, ret;

	if (dev_get_platdata(&pdev->dev) || !np)

		return 0;

	par->clk_count = of_clk_get_parent_count(np);

	if (par->clk_count <= 0)

		return 0;

	par->clks = kcalloc(par->clk_count, sizeof(struct clk *), GFP_KERNEL);

	if (!par->clks)

		return -ENOMEM;

	for (i = 0; i < par->clk_count; i++) {

		clock = of_clk_get(np, i);

		if (IS_ERR(clock)) {

			if (PTR_ERR(clock) == -EPROBE_DEFER) {

				while (--i >= 0) {

					if (par->clks[i])

						clk_put(par->clks[i]);

				}

				kfree(par->clks);

				return -EPROBE_DEFER;

			}

			dev_err(&pdev->dev, "%s: clock %d not found: %ld\n",

				__func__, i, PTR_ERR(clock));

			continue;

		}

		par->clks[i] = clock;

	}

	for (i = 0; i < par->clk_count; i++) {

		if (par->clks[i]) {

			ret = clk_prepare_enable(par->clks[i]);

			if (ret) {

				dev_err(&pdev->dev,

					"%s: failed to enable clock %d: %d\n",

					__func__, i, ret);

				clk_put(par->clks[i]);

				par->clks[i] = NULL;

			}

		}

	}

	return 0;

}

static void simplefb_clocks_destroy(struct simplefb_par *par)

{

	int i;

	if (!par->clks)

		return;

	for (i = 0; i < par->clk_count; i++) {

		if (par->clks[i]) {

			clk_disable_unprepare(par->clks[i]);

			clk_put(par->clks[i]);

		}

	}

	kfree(par->clks);

}

#else

static int simplefb_clocks_init(struct simplefb_par *par,

	struct platform_device *pdev) { return 0; }

static void simplefb_clocks_destroy(struct simplefb_par *par) { }

#endif

static int simplefb_probe(struct platform_device *pdev)

{

	int ret;

	}

	info->pseudo_palette = par->palette;

	ret = simplefb_clocks_init(par, pdev);

	if (ret < 0)

		goto error_unmap;

	dev_info(&pdev->dev, "framebuffer at 0x%lx, 0x%x bytes, mapped to 0x%p\n",

			     info->fix.smem_start, info->fix.smem_len,

			     info->screen_base);

	ret = register_framebuffer(info);

	if (ret < 0) {

		dev_err(&pdev->dev, "Unable to register simplefb: %d\n", ret);

		goto error_unmap;

		goto error_clocks;

	}

	dev_info(&pdev->dev, "fb%d: simplefb registered!\n", info->node);

	return 0;

error_clocks:

	simplefb_clocks_destroy(par);

error_unmap:

	iounmap(info->screen_base);

error_fb_release:

static int simplefb_remove(struct platform_device *pdev)

{

	struct fb_info *info = platform_get_drvdata(pdev);

	struct simplefb_par *par = info->par;

	unregister_framebuffer(info);

	simplefb_clocks_destroy(par);

	framebuffer_release(info);

	return 0;