OMAP2/3 clock: implement clock rate change notifiers

index 8d43d78..4c2ed56 100644
@@ -21,6 +21,7 @@
 #include <linux/mutex.h>
 #include <linux/platform_device.h>
 #include <linux/cpufreq.h>
+#include <linux/notifier.h>
 #include <linux/debugfs.h>
 #include <linux/io.h>
 #include <linux/bootmem.h>

@@ -34,6 +35,8 @@ static DEFINE_SPINLOCK(clockfw_lock);
 
 static struct clk_functions *arch_clock;
 
+static LIST_HEAD(clk_notifier_list);
+
 /**
  * omap_clk_for_each_child - call callback on each child clock of clk
  * @clk: struct clk * to use as the "parent"

@@ -526,6 +529,125 @@ void clk_init_cpufreq_table(struct cpufreq_frequency_table **table)
 EXPORT_SYMBOL(clk_init_cpufreq_table);
 #endif
 
+/**
+ * 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 an
+ * atomic notifier.  The callback will be called with interrupts
+ * disabled; therefore callback code should be very lightweight.
+ * Callback code must not call back into the clock framework.
+ * Callback code 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 atomic_notifier_chain_register().
+ */
+int clk_notifier_register(struct clk *clk, struct notifier_block *nb)
+{
+	struct clk_notifier *cn = NULL, *cn_new = NULL;
+	int r;
+	unsigned long flags;
+	struct clk *clkp;
+
+	if (!clk || !nb)
+		return -EINVAL;
+
+	/* Allocate this here speculatively so we can avoid GFP_ATOMIC */
+	cn_new = kzalloc(sizeof(struct clk_notifier), GFP_KERNEL);
+	if (!cn_new)
+		return -ENOMEM;
+
+	spin_lock_irqsave(&clockfw_lock, flags);
+
+	list_for_each_entry(cn, &clk_notifier_list, node) {
+		if (cn->clk == clk)
+			break;
+	}
+
+	if (cn->clk != clk) {
+		cn_new->clk = clk;
+		ATOMIC_INIT_NOTIFIER_HEAD(&cn_new->notifier_head);
+
+		list_add(&cn_new->node, &clk_notifier_list);
+		cn = cn_new;
+	} else {
+		kfree(cn_new); /* didn't need it after all */
+	}
+
+	r = atomic_notifier_chain_register(&cn->notifier_head, nb);
+	if (!r) {
+		clkp = clk;
+		do {
+			clkp->notifier_count++;
+		} while ((clkp = clkp->parent));
+	}
+
+	spin_unlock_irqrestore(&clockfw_lock, flags);
+
+	return r;
+}
+
+/**
+ * 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'.  This
+ * function presently does not release memory allocated by its
+ * corresponding _register function; see inline comments for more
+ * information.  Returns -EINVAL if called with null arguments;
+ * otherwise, passes along the return value of
+ * atomic_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;
+	unsigned long flags;
+
+	if (!clk || !nb)
+		return -EINVAL;
+
+	spin_lock_irqsave(&clockfw_lock, flags);
+
+	list_for_each_entry(cn, &clk_notifier_list, node) {
+		if (cn->clk == clk)
+			break;
+	}
+
+	if (cn->clk == clk) {
+		r = atomic_notifier_chain_unregister(&cn->notifier_head, nb);
+
+		if (!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)
+			kfree(cn);
+
+	} else {
+		r = -ENOENT;
+	}
+
+	spin_unlock_irqrestore(&clockfw_lock, flags);
+
+	return r;
+}
+
+
+
 /*-------------------------------------------------------------------------*/
 
 #ifdef CONFIG_OMAP_RESET_CLOCKS

110 lines not on this page · raw message

index 4c2ed56..73bef03 100644
@@ -173,6 +173,64 @@ void omap_clk_del_child(struct clk *clk, struct clk *clk2)
 	}
 }
 
+/**
+ * omap_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 int omap_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;
+	int ret = NOTIFY_DONE;
+
+	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) {
+			ret = atomic_notifier_call_chain(&cn->notifier_head,
+							 msg, &cnd);
+			break;
+		}
+	}
+
+	return ret;
+}
+
+/**
+ * _do_clk_notifier - callback function for clock change notification
+ * @clk: struct clk * to start the notifications with
+ * @msg: notifier msg - see "Clk notifier callback types"
+ * @param2: unused
+ *
+ * Notify callbacks associated with @clk that a clock change will or has
+ * occurred.  If @msg is CLK_PREPARE_RATE_CHANGE, then actually pay attention
+ * to the notifier return value.
+ */
+static int _do_clk_notifier(struct clk *clk, unsigned long msg, u8 param2)
+{
+	int ret;
+
+	ret = omap_clk_notify(clk, msg, clk->rate, clk->temp_rate);
+	if (ret && msg == CLK_PREPARE_RATE_CHANGE)
+		return ret;
+
+	if (omap_clk_has_children(clk))
+		return omap_clk_notify_downstream(clk, msg);
+	else
+		return 0;
+}
+
 /*-------------------------------------------------------------------------
  * Standard clock functions defined in include/linux/clk.h
  *-------------------------------------------------------------------------*/

@@ -440,6 +498,27 @@ void recalculate_root_clocks(void)
 			_do_propagate_rate(clkp, 0, CURRENT_RATE);
 }
 
+/**
+ * omap_clk_notify_downstream - trigger clock change notifications
+ * @clk: struct clk * to start the notifications with
+ * @msg: notifier msg - see "Clk notifier callback types"
+ *
+ * Call clock change notifiers on clocks starting with @clk and including
+ * all of @clk's downstream children clocks.  Returns NOTIFY_DONE if
+ * the notifiers ran successfully, or when msg is CLK_PREPARE_RATE_CHANGE,
+ * NOTIFY_BAD if one of the notifiers denied the change.
+ */
+int omap_clk_notify_downstream(struct clk *clk, unsigned long msg)
+{
+	if (clk == NULL || IS_ERR(clk))
+		return -EINVAL;
+
+	if (!clk->notifier_count)
+		return 0;
+
+	return omap_clk_for_each_child(clk, msg, 0, _do_clk_notifier);
+}
+
 int clk_register(struct clk *clk)
 {
 	if (clk == NULL || IS_ERR(clk))

@@ -529,6 +608,8 @@ void clk_init_cpufreq_table(struct cpufreq_frequency_table **table)
 EXPORT_SYMBOL(clk_init_cpufreq_table);
 #endif
 
+/* Clk notifier implementations */
+
 /**
  * clk_notifier_register - add a clock parameter change notifier
  * @clk: struct clk * to watch

index d08f16c..2faf4c4 100644
@@ -183,6 +183,7 @@ extern void clk_init_cpufreq_table(struct cpufreq_frequency_table **table);
 #endif
 void omap_clk_add_child(struct clk *clk, struct clk *clk2);
 void omap_clk_del_child(struct clk *clk, struct clk *clk2);
+extern int omap_clk_notify_downstream(struct clk *clk, unsigned long msg);
 
 /* Clock flags */
 #define RATE_CKCTL		(1 << 0)	/* Main fixed ratio clocks */

index c21767a..c990a17 100644
@@ -790,10 +790,18 @@ int omap2_clk_set_rate(struct clk *clk, unsigned long rate)
 {
 	int ret = -EINVAL;
 
-	pr_debug("clock: set_rate for clock %s to rate %ld\n", clk->name, rate);
+	if (!clk->set_rate)
+		return -EINVAL;
+
+	if (clk->notifier_count) {
+		clk->temp_rate = rate;
+		propagate_rate(clk, TEMP_RATE);
+	}
+
+	pr_debug("clock: %s: set_rate from %ld Hz to %ld Hz\n", clk->name,
+		 clk->rate, rate);
 
-	if (clk->set_rate != NULL)
-		ret = clk->set_rate(clk, rate);
+	ret = clk->set_rate(clk, rate);
 
 	return ret;
 }

@@ -835,6 +843,7 @@ static u32 _omap2_clksel_get_src_field(struct clk *src_clk, struct clk *clk,
 int omap2_clk_set_parent(struct clk *clk, struct clk *new_parent)
 {
 	u32 field_val, v, parent_div;
+	unsigned long orig_rate, new_rate;
 
 	if (!clk->clksel)
 		return -EINVAL;

@@ -843,6 +852,16 @@ int omap2_clk_set_parent(struct clk *clk, struct clk *new_parent)
 	if (!parent_div)
 		return -EINVAL;
 
+	orig_rate = clk->rate;
+	new_rate = new_parent->rate;
+	if (parent_div > 0)
+		new_rate /= parent_div;
+
+	if (clk->notifier_count) {
+		clk->temp_rate = new_rate;
+		propagate_rate(clk, TEMP_RATE);
+	}
+
 	if (clk->usecount > 0)
 		_omap2_clk_disable(clk);
 

@@ -859,15 +878,10 @@ int omap2_clk_set_parent(struct clk *clk, struct clk *new_parent)
 		_omap2_clk_enable(clk);
 
 	clk->parent = new_parent;
+	clk->rate = new_rate;
 
-	/* CLKSEL clocks follow their parents' rates, divided by a divisor */
-	clk->rate = new_parent->rate;
-
-	if (parent_div > 0)
-		clk->rate /= parent_div;
-
-	pr_debug("clock: set parent of %s to %s (new rate %ld)\n",
-		 clk->name, clk->parent->name, clk->rate);
+	pr_debug("clock: %s: set parent to %s (orig rate %ld, new rate %ld)\n",
+		 clk->name, clk->parent->name, orig_rate, new_rate);
 
 	return 0;
 }

index c990a17..843d1b5 100644
@@ -789,6 +789,7 @@ int omap2_clksel_set_rate(struct clk *clk, unsigned long rate)
 int omap2_clk_set_rate(struct clk *clk, unsigned long rate)
 {
 	int ret = -EINVAL;
+	unsigned long temp_rate;
 
 	if (!clk->set_rate)
 		return -EINVAL;

@@ -803,6 +804,11 @@ int omap2_clk_set_rate(struct clk *clk, unsigned long rate)
 
 	ret = clk->set_rate(clk, rate);
 
+	temp_rate = clk->rate;
+	clk->rate = clk->temp_rate;
+	omap_clk_notify_downstream(clk, CLK_POST_RATE_CHANGE);
+	clk->rate = temp_rate;
+
 	return ret;
 }
 

@@ -878,11 +884,14 @@ int omap2_clk_set_parent(struct clk *clk, struct clk *new_parent)
 		_omap2_clk_enable(clk);
 
 	clk->parent = new_parent;
-	clk->rate = new_rate;
 
 	pr_debug("clock: %s: set parent to %s (orig rate %ld, new rate %ld)\n",
 		 clk->name, clk->parent->name, orig_rate, new_rate);
 
+	omap_clk_notify_downstream(clk, CLK_POST_RATE_CHANGE);
+
+	clk->rate = new_rate;
+
 	return 0;
 }
 

index 843d1b5..2a024bf 100644
@@ -802,6 +802,8 @@ int omap2_clk_set_rate(struct clk *clk, unsigned long rate)
 	pr_debug("clock: %s: set_rate from %ld Hz to %ld Hz\n", clk->name,
 		 clk->rate, rate);
 
+	omap_clk_notify_downstream(clk, CLK_PRE_RATE_CHANGE);
+
 	ret = clk->set_rate(clk, rate);
 
 	temp_rate = clk->rate;

@@ -868,6 +870,8 @@ int omap2_clk_set_parent(struct clk *clk, struct clk *new_parent)
 		propagate_rate(clk, TEMP_RATE);
 	}
 
+	omap_clk_notify_downstream(clk, CLK_PRE_RATE_CHANGE);
+
 	if (clk->usecount > 0)
 		_omap2_clk_disable(clk);
 

index 2a024bf..50d8e06 100644
@@ -790,6 +790,7 @@ int omap2_clk_set_rate(struct clk *clk, unsigned long rate)
 {
 	int ret = -EINVAL;
 	unsigned long temp_rate;
+	int r;
 
 	if (!clk->set_rate)
 		return -EINVAL;

@@ -799,6 +800,13 @@ int omap2_clk_set_rate(struct clk *clk, unsigned long rate)
 		propagate_rate(clk, TEMP_RATE);
 	}
 
+	r = omap_clk_notify_downstream(clk, CLK_PREPARE_RATE_CHANGE);
+	if (r == NOTIFY_BAD) {
+		pr_debug("clock: %s: clk_set_rate() aborted by notifier\n",
+			 clk->name);
+		return -EAGAIN;
+	}
+
 	pr_debug("clock: %s: set_rate from %ld Hz to %ld Hz\n", clk->name,
 		 clk->rate, rate);
 

@@ -851,6 +859,7 @@ static u32 _omap2_clksel_get_src_field(struct clk *src_clk, struct clk *clk,
 int omap2_clk_set_parent(struct clk *clk, struct clk *new_parent)
 {
 	u32 field_val, v, parent_div;
+	int r;
 	unsigned long orig_rate, new_rate;
 
 	if (!clk->clksel)

@@ -870,6 +879,13 @@ int omap2_clk_set_parent(struct clk *clk, struct clk *new_parent)
 		propagate_rate(clk, TEMP_RATE);
 	}
 
+	r = omap_clk_notify_downstream(clk, CLK_PREPARE_RATE_CHANGE);
+	if (r == NOTIFY_BAD) {
+		pr_debug("clock: %s: clk_set_parent() aborted by notifier\n",
+			 clk->name);
+		return -EAGAIN;
+	}
+
 	omap_clk_notify_downstream(clk, CLK_PRE_RATE_CHANGE);
 
 	if (clk->usecount > 0)

index 50d8e06..1600119 100644
@@ -804,6 +804,7 @@ int omap2_clk_set_rate(struct clk *clk, unsigned long rate)
 	if (r == NOTIFY_BAD) {
 		pr_debug("clock: %s: clk_set_rate() aborted by notifier\n",
 			 clk->name);
+		omap_clk_notify_downstream(clk, CLK_ABORT_RATE_CHANGE);
 		return -EAGAIN;
 	}
 

@@ -814,10 +815,16 @@ int omap2_clk_set_rate(struct clk *clk, unsigned long rate)
 
 	ret = clk->set_rate(clk, rate);
 
-	temp_rate = clk->rate;
-	clk->rate = clk->temp_rate;
-	omap_clk_notify_downstream(clk, CLK_POST_RATE_CHANGE);
-	clk->rate = temp_rate;
+	if (ret == 0) {
+		temp_rate = clk->rate;
+		clk->rate = clk->temp_rate;
+		omap_clk_notify_downstream(clk, CLK_POST_RATE_CHANGE);
+		clk->rate = temp_rate;
+	} else {
+		pr_debug("clock: %s: clk_set_rate() aborted by failed "
+			 "set_rate(): %d\n", clk->name, ret);
+		omap_clk_notify_downstream(clk, CLK_ABORT_RATE_CHANGE);
+	}
 
 	return ret;
 }

@@ -883,6 +890,7 @@ int omap2_clk_set_parent(struct clk *clk, struct clk *new_parent)
 	if (r == NOTIFY_BAD) {
 		pr_debug("clock: %s: clk_set_parent() aborted by notifier\n",
 			 clk->name);
+		omap_clk_notify_downstream(clk, CLK_ABORT_RATE_CHANGE);
 		return -EAGAIN;
 	}