From 6cff575b83ad23e7549640ac7685d7e2eb0657be Mon Sep 17 00:00:00 2001
From: dkl <dkl@rock-chips.com>
Date: Fri, 9 May 2014 14:47:47 +0800
Subject: [PATCH] clk: pd: rockchip: add notify when clk_pd_enable/disable

---
 drivers/clk/rockchip/clk-pd.c | 125 +++++++++++++++++++++++++++++++++-
 drivers/clk/rockchip/clk-pd.h |  15 ++++
 2 files changed, 138 insertions(+), 2 deletions(-)

diff --git a/drivers/clk/rockchip/clk-pd.c b/drivers/clk/rockchip/clk-pd.c
index cf58abadf08a..6a2a79da50b4 100644
--- a/drivers/clk/rockchip/clk-pd.c
+++ b/drivers/clk/rockchip/clk-pd.c
@@ -4,6 +4,98 @@
 #include "clk-pd.h"
 
 
+static LIST_HEAD(clk_pd_notifier_list);
+
+static int __clk_pd_notify(struct clk *clk, unsigned long msg)
+{
+	struct clk_pd_notifier *cn;
+	int ret = NOTIFY_DONE;
+
+	list_for_each_entry(cn, &clk_pd_notifier_list, node) {
+		if (cn->clk == clk) {
+			ret = srcu_notifier_call_chain(&cn->notifier_head, msg,
+					NULL);
+			break;
+		}
+	}
+
+	return ret;
+}
+
+int rk_clk_pd_notifier_register(struct clk *clk, struct notifier_block *nb)
+{
+	struct clk_pd_notifier *cn;
+	int ret = -ENOMEM;
+
+	if (!clk || !nb)
+		return -EINVAL;
+
+	//clk_prepare_lock();
+
+	/* search the list of notifiers for this clk */
+	list_for_each_entry(cn, &clk_pd_notifier_list, node)
+		if (cn->clk == clk)
+			break;
+
+	/* if clk wasn't in the notifier list, allocate new clk_notifier */
+	if (cn->clk != clk) {
+		cn = kzalloc(sizeof(struct clk_pd_notifier), GFP_KERNEL);
+		if (!cn)
+			goto out;
+
+		cn->clk = clk;
+		srcu_init_notifier_head(&cn->notifier_head);
+
+		list_add(&cn->node, &clk_pd_notifier_list);
+	}
+
+	ret = srcu_notifier_chain_register(&cn->notifier_head, nb);
+
+	//clk->notifier_count++;
+
+out:
+	//clk_prepare_unlock();
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(rk_clk_pd_notifier_register);
+
+int rk_clk_pd_notifier_unregister(struct clk *clk, struct notifier_block *nb)
+{
+	struct clk_pd_notifier *cn = NULL;
+	int ret = -EINVAL;
+
+	if (!clk || !nb)
+		return -EINVAL;
+
+	//clk_prepare_lock();
+
+	list_for_each_entry(cn, &clk_pd_notifier_list, node)
+		if (cn->clk == clk)
+			break;
+
+	if (cn->clk == clk) {
+		ret = srcu_notifier_chain_unregister(&cn->notifier_head, nb);
+
+		//clk->notifier_count--;
+
+		/* XXX the notifier code should handle this better */
+		if (!cn->notifier_head.head) {
+			srcu_cleanup_notifier_head(&cn->notifier_head);
+			list_del(&cn->node);
+			kfree(cn);
+		}
+
+	} else {
+		ret = -ENOENT;
+	}
+
+	//clk_prepare_unlock();
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(rk_clk_pd_notifier_unregister);
+
 static int clk_pd_endisable(struct clk_hw *hw, bool enable)
 {
 	struct clk_pd *pd = to_clk_pd(hw);
@@ -23,12 +115,24 @@ static int clk_pd_endisable(struct clk_hw *hw, bool enable)
 
 static int clk_pd_enable(struct clk_hw *hw)
 {
-	return clk_pd_endisable(hw, true);
+	int ret = 0;
+
+	__clk_pd_notify(hw->clk, RK_CLK_PD_PRE_ENABLE);
+
+	ret = clk_pd_endisable(hw, true);
+
+	__clk_pd_notify(hw->clk, RK_CLK_PD_POST_ENABLE);
+
+	return ret;
 }
 
 static void clk_pd_disable(struct clk_hw *hw)
 {
+	__clk_pd_notify(hw->clk, RK_CLK_PD_PRE_DISABLE);
+
 	clk_pd_endisable(hw, false);
+
+	__clk_pd_notify(hw->clk, RK_CLK_PD_POST_DISABLE);
 }
 
 static int clk_pd_is_enabled(struct clk_hw *hw)
@@ -44,8 +148,25 @@ const struct clk_ops clk_pd_ops = {
 	.is_enabled = clk_pd_is_enabled,
 };
 
-const struct clk_ops clk_pd_virt_ops = {
+static int clk_pd_virt_enable(struct clk_hw *hw)
+{
+	__clk_pd_notify(hw->clk, RK_CLK_PD_PRE_ENABLE);
+
+	__clk_pd_notify(hw->clk, RK_CLK_PD_POST_ENABLE);
+
+	return 0;
+}
+
+static void clk_pd_virt_disable(struct clk_hw *hw)
+{
+	__clk_pd_notify(hw->clk, RK_CLK_PD_PRE_DISABLE);
 
+	__clk_pd_notify(hw->clk, RK_CLK_PD_POST_DISABLE);
+}
+
+const struct clk_ops clk_pd_virt_ops = {
+	.enable = clk_pd_virt_enable,
+	.disable = clk_pd_virt_disable,
 };
 
 
diff --git a/drivers/clk/rockchip/clk-pd.h b/drivers/clk/rockchip/clk-pd.h
index 7f94516384c9..650fc04e7472 100644
--- a/drivers/clk/rockchip/clk-pd.h
+++ b/drivers/clk/rockchip/clk-pd.h
@@ -19,4 +19,19 @@ struct clk *rk_clk_register_pd(struct device *dev, const char *name,
 		u32 pd_id, spinlock_t *lock);
 
 
+#define RK_CLK_PD_PRE_ENABLE			BIT(0)
+#define RK_CLK_PD_POST_ENABLE			BIT(1)
+#define RK_CLK_PD_PRE_DISABLE			BIT(2)
+#define RK_CLK_PD_POST_DISABLE			BIT(3)
+
+struct clk_pd_notifier {
+	struct clk			*clk;
+	struct srcu_notifier_head	notifier_head;
+	struct list_head		node;
+};
+
+int rk_clk_pd_notifier_register(struct clk *clk, struct notifier_block *nb);
+
+int rk_clk_pd_notifier_unregister(struct clk *clk, struct notifier_block *nb);
+
 #endif /* __RK_CLK_PD_H */
-- 
2.34.1