[linux-2.6-block.git] / drivers / media / v4l2-core / v4l2-clk.c

/*
 * V4L2 clock service
 *
 * Copyright (C) 2012-2013, Guennadi Liakhovetski <g.liakhovetski@gmx.de>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 */

#include <linux/atomic.h>
#include <linux/clk.h>
#include <linux/device.h>
#include <linux/errno.h>
#include <linux/list.h>
#include <linux/module.h>
#include <linux/mutex.h>
#include <linux/of.h>
#include <linux/slab.h>
#include <linux/string.h>

#include <media/v4l2-clk.h>
#include <media/v4l2-subdev.h>

static DEFINE_MUTEX(clk_lock);
static LIST_HEAD(clk_list);

static struct v4l2_clk *v4l2_clk_find(const char *dev_id)
{
	struct v4l2_clk *clk;

	list_for_each_entry(clk, &clk_list, list)
		if (!strcmp(dev_id, clk->dev_id))
			return clk;

	return ERR_PTR(-ENODEV);
}

struct v4l2_clk *v4l2_clk_get(struct device *dev, const char *id)
{
	struct v4l2_clk *clk;
	struct clk *ccf_clk = clk_get(dev, id);
	char clk_name[V4L2_CLK_NAME_SIZE];

	if (PTR_ERR(ccf_clk) == -EPROBE_DEFER)
		return ERR_PTR(-EPROBE_DEFER);

	if (!IS_ERR_OR_NULL(ccf_clk)) {
		clk = kzalloc(sizeof(*clk), GFP_KERNEL);
		if (!clk) {
			clk_put(ccf_clk);
			return ERR_PTR(-ENOMEM);
		}
		clk->clk = ccf_clk;

		return clk;
	}

	mutex_lock(&clk_lock);
	clk = v4l2_clk_find(dev_name(dev));

	/* if dev_name is not found, try use the OF name to find again  */
	if (PTR_ERR(clk) == -ENODEV && dev->of_node) {
		v4l2_clk_name_of(clk_name, sizeof(clk_name), dev->of_node);
		clk = v4l2_clk_find(clk_name);
	}

	if (!IS_ERR(clk))
		atomic_inc(&clk->use_count);
	mutex_unlock(&clk_lock);

	return clk;
}
EXPORT_SYMBOL(v4l2_clk_get);

void v4l2_clk_put(struct v4l2_clk *clk)
{
	struct v4l2_clk *tmp;

	if (IS_ERR(clk))
		return;

	if (clk->clk) {
		clk_put(clk->clk);
		kfree(clk);
		return;
	}

	mutex_lock(&clk_lock);

	list_for_each_entry(tmp, &clk_list, list)
		if (tmp == clk)
			atomic_dec(&clk->use_count);

	mutex_unlock(&clk_lock);
}
EXPORT_SYMBOL(v4l2_clk_put);

static int v4l2_clk_lock_driver(struct v4l2_clk *clk)
{
	struct v4l2_clk *tmp;
	int ret = -ENODEV;

	mutex_lock(&clk_lock);

	list_for_each_entry(tmp, &clk_list, list)
		if (tmp == clk) {
			ret = !try_module_get(clk->ops->owner);
			if (ret)
				ret = -EFAULT;
			break;
		}

	mutex_unlock(&clk_lock);

	return ret;
}

static void v4l2_clk_unlock_driver(struct v4l2_clk *clk)
{
	module_put(clk->ops->owner);
}

int v4l2_clk_enable(struct v4l2_clk *clk)
{
	int ret;

	if (clk->clk)
		return clk_prepare_enable(clk->clk);

	ret = v4l2_clk_lock_driver(clk);
	if (ret < 0)
		return ret;

	mutex_lock(&clk->lock);

	if (++clk->enable == 1 && clk->ops->enable) {
		ret = clk->ops->enable(clk);
		if (ret < 0)
			clk->enable--;
	}

	mutex_unlock(&clk->lock);

	return ret;
}
EXPORT_SYMBOL(v4l2_clk_enable);

/*
 * You might Oops if you try to disabled a disabled clock, because then the
 * driver isn't locked and could have been unloaded by now, so, don't do that
 */
void v4l2_clk_disable(struct v4l2_clk *clk)
{
	int enable;

	if (clk->clk)
		return clk_disable_unprepare(clk->clk);

	mutex_lock(&clk->lock);

	enable = --clk->enable;
	if (WARN(enable < 0, "Unbalanced %s() on %s!\n", __func__,
		 clk->dev_id))
		clk->enable++;
	else if (!enable && clk->ops->disable)
		clk->ops->disable(clk);

	mutex_unlock(&clk->lock);

	v4l2_clk_unlock_driver(clk);
}
EXPORT_SYMBOL(v4l2_clk_disable);

unsigned long v4l2_clk_get_rate(struct v4l2_clk *clk)
{
	int ret;

	if (clk->clk)
		return clk_get_rate(clk->clk);

	ret = v4l2_clk_lock_driver(clk);
	if (ret < 0)
		return ret;

	mutex_lock(&clk->lock);
	if (!clk->ops->get_rate)
		ret = -ENOSYS;
	else
		ret = clk->ops->get_rate(clk);
	mutex_unlock(&clk->lock);

	v4l2_clk_unlock_driver(clk);

	return ret;
}
EXPORT_SYMBOL(v4l2_clk_get_rate);

int v4l2_clk_set_rate(struct v4l2_clk *clk, unsigned long rate)
{
	int ret;

	if (clk->clk) {
		long r = clk_round_rate(clk->clk, rate);
		if (r < 0)
			return r;
		return clk_set_rate(clk->clk, r);
	}

	ret = v4l2_clk_lock_driver(clk);

	if (ret < 0)
		return ret;

	mutex_lock(&clk->lock);
	if (!clk->ops->set_rate)
		ret = -ENOSYS;
	else
		ret = clk->ops->set_rate(clk, rate);
	mutex_unlock(&clk->lock);

	v4l2_clk_unlock_driver(clk);

	return ret;
}
EXPORT_SYMBOL(v4l2_clk_set_rate);

struct v4l2_clk *v4l2_clk_register(const struct v4l2_clk_ops *ops,
				   const char *dev_id,
				   void *priv)
{
	struct v4l2_clk *clk;
	int ret;

	if (!ops || !dev_id)
		return ERR_PTR(-EINVAL);

	clk = kzalloc(sizeof(struct v4l2_clk), GFP_KERNEL);
	if (!clk)
		return ERR_PTR(-ENOMEM);

	clk->dev_id = kstrdup(dev_id, GFP_KERNEL);
	if (!clk->dev_id) {
		ret = -ENOMEM;
		goto ealloc;
	}
	clk->ops = ops;
	clk->priv = priv;
	atomic_set(&clk->use_count, 0);
	mutex_init(&clk->lock);

	mutex_lock(&clk_lock);
	if (!IS_ERR(v4l2_clk_find(dev_id))) {
		mutex_unlock(&clk_lock);
		ret = -EEXIST;
		goto eexist;
	}
	list_add_tail(&clk->list, &clk_list);
	mutex_unlock(&clk_lock);

	return clk;

eexist:
ealloc:
	kfree(clk->dev_id);
	kfree(clk);
	return ERR_PTR(ret);
}
EXPORT_SYMBOL(v4l2_clk_register);

void v4l2_clk_unregister(struct v4l2_clk *clk)
{
	if (WARN(atomic_read(&clk->use_count),
		 "%s(): Refusing to unregister ref-counted %s clock!\n",
		 __func__, clk->dev_id))
		return;

	mutex_lock(&clk_lock);
	list_del(&clk->list);
	mutex_unlock(&clk_lock);

	kfree(clk->dev_id);
	kfree(clk);
}
EXPORT_SYMBOL(v4l2_clk_unregister);

struct v4l2_clk_fixed {
	unsigned long rate;
	struct v4l2_clk_ops ops;
};

static unsigned long fixed_get_rate(struct v4l2_clk *clk)
{
	struct v4l2_clk_fixed *priv = clk->priv;
	return priv->rate;
}

struct v4l2_clk *__v4l2_clk_register_fixed(const char *dev_id,
				unsigned long rate, struct module *owner)
{
	struct v4l2_clk *clk;
	struct v4l2_clk_fixed *priv = kzalloc(sizeof(*priv), GFP_KERNEL);

	if (!priv)
		return ERR_PTR(-ENOMEM);

	priv->rate = rate;
	priv->ops.get_rate = fixed_get_rate;
	priv->ops.owner = owner;

	clk = v4l2_clk_register(&priv->ops, dev_id, priv);
	if (IS_ERR(clk))
		kfree(priv);

	return clk;
}
EXPORT_SYMBOL(__v4l2_clk_register_fixed);

void v4l2_clk_unregister_fixed(struct v4l2_clk *clk)
{
	kfree(clk->priv);
	v4l2_clk_unregister(clk);
}
EXPORT_SYMBOL(v4l2_clk_unregister_fixed);
Commit	Line	Data
ff5430de GL	1	/*
	2	* V4L2 clock service
	3	*
	4	* Copyright (C) 2012-2013, Guennadi Liakhovetski <g.liakhovetski@gmx.de>
	5	*
	6	* This program is free software; you can redistribute it and/or modify
	7	* it under the terms of the GNU General Public License version 2 as
	8	* published by the Free Software Foundation.
	9	*/
	10
	11	#include <linux/atomic.h>
4f528afc	12	#include <linux/clk.h>
ff5430de GL	13	#include <linux/device.h>
	14	#include <linux/errno.h>
	15	#include <linux/list.h>
	16	#include <linux/module.h>
	17	#include <linux/mutex.h>
dc51b7d0	18	#include <linux/of.h>
ff5430de GL	19	#include <linux/slab.h>
	20	#include <linux/string.h>
	21
	22	#include <media/v4l2-clk.h>
	23	#include <media/v4l2-subdev.h>
	24
	25	static DEFINE_MUTEX(clk_lock);
	26	static LIST_HEAD(clk_list);
	27
a37462b9	28	static struct v4l2_clk v4l2_clk_find(const char dev_id)
ff5430de GL	29	{
	30	struct v4l2_clk *clk;
	31
a37462b9 GL	32	list_for_each_entry(clk, &clk_list, list)
a37462b9 GL	33	if (!strcmp(dev_id, clk->dev_id))
ff5430de	34	return clk;
ff5430de GL	35
	36	return ERR_PTR(-ENODEV);
	37	}
	38
	39	struct v4l2_clk v4l2_clk_get(struct device dev, const char *id)
	40	{
	41	struct v4l2_clk *clk;
4f528afc	42	struct clk *ccf_clk = clk_get(dev, id);
dc51b7d0	43	char clk_name[V4L2_CLK_NAME_SIZE];
4f528afc GL	44
	45	if (PTR_ERR(ccf_clk) == -EPROBE_DEFER)
	46	return ERR_PTR(-EPROBE_DEFER);
	47
	48	if (!IS_ERR_OR_NULL(ccf_clk)) {
	49	clk = kzalloc(sizeof(*clk), GFP_KERNEL);
	50	if (!clk) {
	51	clk_put(ccf_clk);
	52	return ERR_PTR(-ENOMEM);
	53	}
	54	clk->clk = ccf_clk;
	55
	56	return clk;
	57	}
ff5430de GL	58
ff5430de GL	59	mutex_lock(&clk_lock);
a37462b9	60	clk = v4l2_clk_find(dev_name(dev));
ff5430de	61
dc51b7d0 JW	62	/* if dev_name is not found, try use the OF name to find again */
dc51b7d0 JW	63	if (PTR_ERR(clk) == -ENODEV && dev->of_node) {
68d9c47b	64	v4l2_clk_name_of(clk_name, sizeof(clk_name), dev->of_node);
dc51b7d0 JW	65	clk = v4l2_clk_find(clk_name);
	66	}
	67
ff5430de GL	68	if (!IS_ERR(clk))
	69	atomic_inc(&clk->use_count);
	70	mutex_unlock(&clk_lock);
	71
	72	return clk;
	73	}
	74	EXPORT_SYMBOL(v4l2_clk_get);
	75
	76	void v4l2_clk_put(struct v4l2_clk *clk)
	77	{
	78	struct v4l2_clk *tmp;
	79
	80	if (IS_ERR(clk))
	81	return;
	82
4f528afc GL	83	if (clk->clk) {
	84	clk_put(clk->clk);
	85	kfree(clk);
	86	return;
	87	}
	88
ff5430de GL	89	mutex_lock(&clk_lock);
	90
	91	list_for_each_entry(tmp, &clk_list, list)
	92	if (tmp == clk)
	93	atomic_dec(&clk->use_count);
	94
	95	mutex_unlock(&clk_lock);
	96	}
	97	EXPORT_SYMBOL(v4l2_clk_put);
	98
	99	static int v4l2_clk_lock_driver(struct v4l2_clk *clk)
	100	{
	101	struct v4l2_clk *tmp;
	102	int ret = -ENODEV;
	103
	104	mutex_lock(&clk_lock);
	105
	106	list_for_each_entry(tmp, &clk_list, list)
	107	if (tmp == clk) {
	108	ret = !try_module_get(clk->ops->owner);
	109	if (ret)
	110	ret = -EFAULT;
	111	break;
	112	}
	113
	114	mutex_unlock(&clk_lock);
	115
	116	return ret;
	117	}
	118
	119	static void v4l2_clk_unlock_driver(struct v4l2_clk *clk)
	120	{
	121	module_put(clk->ops->owner);
	122	}
	123
	124	int v4l2_clk_enable(struct v4l2_clk *clk)
	125	{
4f528afc GL	126	int ret;
	127
	128	if (clk->clk)
	129	return clk_prepare_enable(clk->clk);
ff5430de	130
4f528afc	131	ret = v4l2_clk_lock_driver(clk);
ff5430de GL	132	if (ret < 0)
	133	return ret;
	134
	135	mutex_lock(&clk->lock);
	136
	137	if (++clk->enable == 1 && clk->ops->enable) {
	138	ret = clk->ops->enable(clk);
	139	if (ret < 0)
	140	clk->enable--;
	141	}
	142
	143	mutex_unlock(&clk->lock);
	144
	145	return ret;
	146	}
	147	EXPORT_SYMBOL(v4l2_clk_enable);
	148
	149	/*
	150	* You might Oops if you try to disabled a disabled clock, because then the
	151	* driver isn't locked and could have been unloaded by now, so, don't do that
	152	*/
	153	void v4l2_clk_disable(struct v4l2_clk *clk)
	154	{
	155	int enable;
	156
4f528afc GL	157	if (clk->clk)
	158	return clk_disable_unprepare(clk->clk);
	159
ff5430de GL	160	mutex_lock(&clk->lock);
	161
	162	enable = --clk->enable;
a37462b9 GL	163	if (WARN(enable < 0, "Unbalanced %s() on %s!\n", __func__,
a37462b9 GL	164	clk->dev_id))
ff5430de GL	165	clk->enable++;
	166	else if (!enable && clk->ops->disable)
	167	clk->ops->disable(clk);
	168
	169	mutex_unlock(&clk->lock);
	170
	171	v4l2_clk_unlock_driver(clk);
	172	}
	173	EXPORT_SYMBOL(v4l2_clk_disable);
	174
	175	unsigned long v4l2_clk_get_rate(struct v4l2_clk *clk)
	176	{
4f528afc GL	177	int ret;
	178
	179	if (clk->clk)
	180	return clk_get_rate(clk->clk);
ff5430de	181
4f528afc	182	ret = v4l2_clk_lock_driver(clk);
ff5430de GL	183	if (ret < 0)
	184	return ret;
	185
	186	mutex_lock(&clk->lock);
	187	if (!clk->ops->get_rate)
	188	ret = -ENOSYS;
	189	else
	190	ret = clk->ops->get_rate(clk);
	191	mutex_unlock(&clk->lock);
	192
	193	v4l2_clk_unlock_driver(clk);
	194
	195	return ret;
	196	}
	197	EXPORT_SYMBOL(v4l2_clk_get_rate);
	198
	199	int v4l2_clk_set_rate(struct v4l2_clk *clk, unsigned long rate)
	200	{
4f528afc GL	201	int ret;
	202
	203	if (clk->clk) {
	204	long r = clk_round_rate(clk->clk, rate);
	205	if (r < 0)
	206	return r;
	207	return clk_set_rate(clk->clk, r);
	208	}
	209
	210	ret = v4l2_clk_lock_driver(clk);
ff5430de GL	211
	212	if (ret < 0)
	213	return ret;
	214
	215	mutex_lock(&clk->lock);
	216	if (!clk->ops->set_rate)
	217	ret = -ENOSYS;
	218	else
	219	ret = clk->ops->set_rate(clk, rate);
	220	mutex_unlock(&clk->lock);
	221
	222	v4l2_clk_unlock_driver(clk);
	223
	224	return ret;
	225	}
	226	EXPORT_SYMBOL(v4l2_clk_set_rate);
	227
	228	struct v4l2_clk v4l2_clk_register(const struct v4l2_clk_ops ops,
	229	const char *dev_id,
a37462b9	230	void *priv)
ff5430de GL	231	{
	232	struct v4l2_clk *clk;
	233	int ret;
	234
	235	if (!ops \|\| !dev_id)
	236	return ERR_PTR(-EINVAL);
	237
	238	clk = kzalloc(sizeof(struct v4l2_clk), GFP_KERNEL);
	239	if (!clk)
	240	return ERR_PTR(-ENOMEM);
	241
ff5430de	242	clk->dev_id = kstrdup(dev_id, GFP_KERNEL);
a37462b9	243	if (!clk->dev_id) {
ff5430de GL	244	ret = -ENOMEM;
	245	goto ealloc;
	246	}
	247	clk->ops = ops;
	248	clk->priv = priv;
	249	atomic_set(&clk->use_count, 0);
	250	mutex_init(&clk->lock);
	251
	252	mutex_lock(&clk_lock);
a37462b9	253	if (!IS_ERR(v4l2_clk_find(dev_id))) {
ff5430de GL	254	mutex_unlock(&clk_lock);
	255	ret = -EEXIST;
	256	goto eexist;
	257	}
	258	list_add_tail(&clk->list, &clk_list);
	259	mutex_unlock(&clk_lock);
	260
	261	return clk;
	262
	263	eexist:
	264	ealloc:
ff5430de GL	265	kfree(clk->dev_id);
	266	kfree(clk);
	267	return ERR_PTR(ret);
	268	}
	269	EXPORT_SYMBOL(v4l2_clk_register);
	270
	271	void v4l2_clk_unregister(struct v4l2_clk *clk)
	272	{
	273	if (WARN(atomic_read(&clk->use_count),
a37462b9 GL	274	"%s(): Refusing to unregister ref-counted %s clock!\n",
a37462b9 GL	275	__func__, clk->dev_id))
ff5430de GL	276	return;
	277
	278	mutex_lock(&clk_lock);
	279	list_del(&clk->list);
	280	mutex_unlock(&clk_lock);
	281
ff5430de GL	282	kfree(clk->dev_id);
	283	kfree(clk);
	284	}
	285	EXPORT_SYMBOL(v4l2_clk_unregister);
cf326dfe GL	286
	287	struct v4l2_clk_fixed {
	288	unsigned long rate;
	289	struct v4l2_clk_ops ops;
	290	};
	291
	292	static unsigned long fixed_get_rate(struct v4l2_clk *clk)
	293	{
	294	struct v4l2_clk_fixed *priv = clk->priv;
	295	return priv->rate;
	296	}
	297
	298	struct v4l2_clk __v4l2_clk_register_fixed(const char dev_id,
a37462b9	299	unsigned long rate, struct module *owner)
cf326dfe GL	300	{
	301	struct v4l2_clk *clk;
	302	struct v4l2_clk_fixed priv = kzalloc(sizeof(priv), GFP_KERNEL);
	303
	304	if (!priv)
	305	return ERR_PTR(-ENOMEM);
	306
	307	priv->rate = rate;
	308	priv->ops.get_rate = fixed_get_rate;
	309	priv->ops.owner = owner;
	310
a37462b9	311	clk = v4l2_clk_register(&priv->ops, dev_id, priv);
cf326dfe GL	312	if (IS_ERR(clk))
	313	kfree(priv);
	314
	315	return clk;
	316	}
	317	EXPORT_SYMBOL(__v4l2_clk_register_fixed);
	318
	319	void v4l2_clk_unregister_fixed(struct v4l2_clk *clk)
	320	{
	321	kfree(clk->priv);
	322	v4l2_clk_unregister(clk);
	323	}
	324	EXPORT_SYMBOL(v4l2_clk_unregister_fixed);