From da9a553f0741e80c5387ec3e9bc2b7db19ef7c41 Mon Sep 17 00:00:00 2001 From: =?utf8?q?=E9=BB=84=E6=B6=9B?= Date: Fri, 26 Aug 2011 20:20:42 +0800 Subject: [PATCH] rk29: clock: support notify --- arch/arm/mach-rk29/clock.c | 216 +++++++++++++++++++++++- arch/arm/mach-rk29/include/mach/clock.h | 74 ++++++++ 2 files changed, 281 insertions(+), 9 deletions(-) create mode 100644 arch/arm/mach-rk29/include/mach/clock.h diff --git a/arch/arm/mach-rk29/clock.c b/arch/arm/mach-rk29/clock.c index f5fa5c8a09f3..41822edd7b6b 100755 --- a/arch/arm/mach-rk29/clock.c +++ b/arch/arm/mach-rk29/clock.c @@ -33,6 +33,7 @@ #include #include #include +#include /* Clock flags */ @@ -61,7 +62,8 @@ struct clk { long (*round_rate)(struct clk *, unsigned long); struct clk* (*get_parent)(struct clk *); /* get clk's parent from the hardware. default is clksel_get_parent if parents present */ int (*set_parent)(struct clk *, struct clk *); /* default is clksel_set_parent if parents present */ - s32 usecount; + s16 usecount; + u16 notifier_count; u8 gate_idx; u8 pll_idx; u8 clksel_con; @@ -73,6 +75,8 @@ struct clk { struct clk **parents; }; +static void clk_notify(struct clk *clk, unsigned long msg, + unsigned long old_rate, unsigned long new_rate); static int clk_enable_nolock(struct clk *clk); static void clk_disable_nolock(struct clk *clk); static int clk_set_rate_nolock(struct clk *clk, unsigned long rate); @@ -2220,13 +2224,16 @@ static int clk_enable_nolock(struct clk *clk) return ret; } - if (clk->mode) { + if (clk->notifier_count) + clk_notify(clk, CLK_PRE_ENABLE, clk->rate, clk->rate); + if (clk->mode) ret = clk->mode(clk, 1); - if (ret) { - if (clk->parent) - clk_disable_nolock(clk->parent); - return ret; - } + if (clk->notifier_count) + clk_notify(clk, ret ? CLK_ABORT_ENABLE : CLK_POST_ENABLE, clk->rate, clk->rate); + if (ret) { + if (clk->parent) + clk_disable_nolock(clk->parent); + return ret; } pr_debug("%s enabled\n", clk->name); } @@ -2259,10 +2266,15 @@ static void clk_disable_nolock(struct clk *clk) } if (--clk->usecount == 0) { + int ret = 0; + if (clk->notifier_count) + clk_notify(clk, CLK_PRE_DISABLE, clk->rate, clk->rate); if (clk->mode) - clk->mode(clk, 0); + ret = clk->mode(clk, 0); + if (clk->notifier_count) + clk_notify(clk, ret ? CLK_ABORT_DISABLE : CLK_POST_DISABLE, clk->rate, clk->rate); pr_debug("%s disabled\n", clk->name); - if (clk->parent) + if (ret == 0 && clk->parent) clk_disable_nolock(clk->parent); } } @@ -2332,6 +2344,7 @@ static void __clk_recalc(struct clk *clk) static int clk_set_rate_nolock(struct clk *clk, unsigned long rate) { int ret; + unsigned long old_rate; if (rate == clk->rate) return 0; @@ -2344,6 +2357,10 @@ static int clk_set_rate_nolock(struct clk *clk, unsigned long rate) if (!clk->set_rate) return -EINVAL; + old_rate = clk->rate; + if (clk->notifier_count) + clk_notify(clk, CLK_PRE_RATE_CHANGE, old_rate, rate); + ret = clk->set_rate(clk, rate); if (ret == 0) { @@ -2351,6 +2368,9 @@ static int clk_set_rate_nolock(struct clk *clk, unsigned long rate) __propagate_rate(clk); } + if (clk->notifier_count) + clk_notify(clk, ret ? CLK_ABORT_RATE_CHANGE : CLK_POST_RATE_CHANGE, old_rate, clk->rate); + return ret; } @@ -2877,3 +2897,181 @@ static int __init clk_proc_init(void) late_initcall(clk_proc_init); #endif /* CONFIG_PROC_FS */ +/* Clk notifier implementation */ + +/** + * struct clk_notifier - associate a clk with a notifier + * @clk: struct clk * to associate the notifier with + * @notifier_head: a atomic_notifier_head for this clk + * @node: linked list pointers + * + * A list of struct clk_notifier is maintained by the notifier code. + * An entry is created whenever code registers the first notifier on a + * particular @clk. Future notifiers on that @clk are added to the + * @notifier_head. + */ +struct clk_notifier { + struct clk *clk; + struct atomic_notifier_head notifier_head; + struct list_head node; +}; + +static LIST_HEAD(clk_notifier_list); + +/** + * _clk_free_notifier_chain - safely remove struct clk_notifier + * @cn: struct clk_notifier * + * + * Removes the struct clk_notifier @cn from the clk_notifier_list and + * frees it. + */ +static void _clk_free_notifier_chain(struct clk_notifier *cn) +{ + list_del(&cn->node); + kfree(cn); +} + +/** + * clk_notify - call clk notifier chain + * @clk: struct clk * that is changing rate + * @msg: clk notifier type (i.e., CLK_POST_RATE_CHANGE; see mach/clock.h) + * @old_rate: old rate + * @new_rate: new rate + * + * Triggers a notifier call chain on the post-clk-rate-change notifier + * for clock 'clk'. Passes a pointer to the struct clk and the + * previous and current rates to the notifier callback. Intended to be + * called by internal clock code only. No return value. + */ +static void clk_notify(struct clk *clk, unsigned long msg, + unsigned long old_rate, unsigned long new_rate) +{ + struct clk_notifier *cn; + struct clk_notifier_data cnd; + + cnd.clk = clk; + cnd.old_rate = old_rate; + cnd.new_rate = new_rate; + + list_for_each_entry(cn, &clk_notifier_list, node) { + if (cn->clk == clk) { + pr_debug("%s msg %lu rate %lu -> %lu\n", clk->name, msg, old_rate, new_rate); + atomic_notifier_call_chain(&cn->notifier_head, msg, &cnd); + break; + } + } +} + +/** + * clk_notifier_register - add a clock parameter change notifier + * @clk: struct clk * to watch + * @nb: struct notifier_block * with callback info + * + * Request notification for changes to the clock 'clk'. This uses a + * blocking notifier. Callback code must not call into the clock + * framework, as clocks_mutex is held. Pre-notifier callbacks will be + * passed the previous and new rate of the clock. + * + * clk_notifier_register() must be called from process + * context. Returns -EINVAL if called with null arguments, -ENOMEM + * upon allocation failure; otherwise, passes along the return value + * of blocking_notifier_chain_register(). + */ +int clk_notifier_register(struct clk *clk, struct notifier_block *nb) +{ + struct clk_notifier *cn = NULL, *cn_new = NULL; + int r; + struct clk *clkp; + + if (!clk || !nb) + return -EINVAL; + + mutex_lock(&clocks_mutex); + + list_for_each_entry(cn, &clk_notifier_list, node) + if (cn->clk == clk) + break; + + if (cn->clk != clk) { + cn_new = kzalloc(sizeof(struct clk_notifier), GFP_KERNEL); + if (!cn_new) { + r = -ENOMEM; + goto cnr_out; + }; + + cn_new->clk = clk; + ATOMIC_INIT_NOTIFIER_HEAD(&cn_new->notifier_head); + + list_add(&cn_new->node, &clk_notifier_list); + cn = cn_new; + } + + r = atomic_notifier_chain_register(&cn->notifier_head, nb); + if (!IS_ERR_VALUE(r)) { + clkp = clk; + do { + clkp->notifier_count++; + } while ((clkp = clkp->parent)); + } else { + if (cn_new) + _clk_free_notifier_chain(cn); + } + +cnr_out: + mutex_unlock(&clocks_mutex); + + return r; +} +EXPORT_SYMBOL(clk_notifier_register); + +/** + * clk_notifier_unregister - remove a clock change notifier + * @clk: struct clk * + * @nb: struct notifier_block * with callback info + * + * Request no further notification for changes to clock 'clk'. + * Returns -EINVAL if called with null arguments; otherwise, passes + * along the return value of blocking_notifier_chain_unregister(). + */ +int clk_notifier_unregister(struct clk *clk, struct notifier_block *nb) +{ + struct clk_notifier *cn = NULL; + struct clk *clkp; + int r = -EINVAL; + + if (!clk || !nb) + return -EINVAL; + + mutex_lock(&clocks_mutex); + + list_for_each_entry(cn, &clk_notifier_list, node) + if (cn->clk == clk) + break; + + if (cn->clk != clk) { + r = -ENOENT; + goto cnu_out; + }; + + r = atomic_notifier_chain_unregister(&cn->notifier_head, nb); + if (!IS_ERR_VALUE(r)) { + clkp = clk; + do { + clkp->notifier_count--; + } while ((clkp = clkp->parent)); + } + + /* + * XXX ugh, layering violation. There should be some + * support in the notifier code for this. + */ + if (!cn->notifier_head.head) + _clk_free_notifier_chain(cn); + +cnu_out: + mutex_unlock(&clocks_mutex); + + return r; +} +EXPORT_SYMBOL(clk_notifier_unregister); + diff --git a/arch/arm/mach-rk29/include/mach/clock.h b/arch/arm/mach-rk29/include/mach/clock.h new file mode 100644 index 000000000000..e0ac1cef3919 --- /dev/null +++ b/arch/arm/mach-rk29/include/mach/clock.h @@ -0,0 +1,74 @@ +/* arch/arm/mach-rk29/include/mach/clock.h + * + * Copyright (C) 2011 ROCKCHIP, Inc. + * + * This software is licensed under the terms of the GNU General Public + * License version 2, as published by the Free Software Foundation, and + * may be copied, distributed, and modified under those terms. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + */ + +#ifndef __ASM_ARCH_RK29_CLOCK_H +#define __ASM_ARCH_RK29_CLOCK_H + +/** + * struct clk_notifier_data - rate data to pass to the notifier callback + * @clk: struct clk * being changed + * @old_rate: previous rate of this clock + * @new_rate: new rate of this clock + * + * For a pre-notifier, old_rate is the clock's rate before this rate + * change, and new_rate is what the rate will be in the future. For a + * post-notifier, old_rate and new_rate are both set to the clock's + * current rate (this was done to optimize the implementation). + */ +struct clk_notifier_data { + struct clk *clk; + unsigned long old_rate; + unsigned long new_rate; +}; + +/* + * Clk notifier callback types + * + * Since the notifier is called with interrupts disabled, any actions + * taken by callbacks must be extremely fast and lightweight. + * + * CLK_PRE_RATE_CHANGE - called after all callbacks have approved the + * rate change, immediately before the clock rate is changed, to + * indicate that the rate change will proceed. Drivers must + * immediately terminate any operations that will be affected by + * the rate change. Callbacks must always return NOTIFY_DONE. + * + * CLK_ABORT_RATE_CHANGE: called if the rate change failed for some + * reason after CLK_PRE_RATE_CHANGE. In this case, all registered + * notifiers on the clock will be called with + * CLK_ABORT_RATE_CHANGE. Callbacks must always return + * NOTIFY_DONE. + * + * CLK_POST_RATE_CHANGE - called after the clock rate change has + * successfully completed. Callbacks must always return + * NOTIFY_DONE. + * + */ +#define CLK_PRE_RATE_CHANGE 1 +#define CLK_POST_RATE_CHANGE 2 +#define CLK_ABORT_RATE_CHANGE 3 + +#define CLK_PRE_ENABLE 4 +#define CLK_POST_ENABLE 5 +#define CLK_ABORT_ENABLE 6 + +#define CLK_PRE_DISABLE 7 +#define CLK_POST_DISABLE 8 +#define CLK_ABORT_DISABLE 9 + +extern int clk_notifier_register(struct clk *clk, struct notifier_block *nb); +extern int clk_notifier_unregister(struct clk *clk, struct notifier_block *nb); + +#endif -- 2.34.1