From 5c27ce1dc05fd740cc8f0f1d8072361ccab51a9f Mon Sep 17 00:00:00 2001
From: Romuald JEANNE <romuald.jeanne@st.com>
Date: Tue, 16 Mar 2021 09:02:36 +0100
Subject: [PATCH 07/22] ARM 5.10.10-stm32mp1-r1 HWSPINLOCK

Signed-off-by: Romuald JEANNE <romuald.jeanne@st.com>
---
 Documentation/locking/hwspinlock.rst     | 10 ++-
 drivers/hwspinlock/hwspinlock_core.c     | 80 +++++++++++++++++++-----
 drivers/hwspinlock/hwspinlock_internal.h |  2 +
 drivers/hwspinlock/stm32_hwspinlock.c    | 60 +++++++++++-------
 4 files changed, 110 insertions(+), 42 deletions(-)

diff --git a/Documentation/locking/hwspinlock.rst b/Documentation/locking/hwspinlock.rst
index 6f03713b7003..605bd2dc8a03 100644
--- a/Documentation/locking/hwspinlock.rst
+++ b/Documentation/locking/hwspinlock.rst
@@ -54,9 +54,11 @@ Should be called from a process context (might sleep).
   struct hwspinlock *hwspin_lock_request_specific(unsigned int id);
 
 Assign a specific hwspinlock id and return its address, or NULL
-if that hwspinlock is already in use. Usually board code will
-be calling this function in order to reserve specific hwspinlock
-ids for predefined purposes.
+if that hwspinlock is already in use and not shared. If that specific
+hwspinlock is declared as shared, it can be requested and used by
+several users.
+Usually board code will be calling this function in order to reserve
+specific hwspinlock ids for predefined purposes.
 
 Should be called from a process context (might sleep).
 
@@ -449,11 +451,13 @@ of which represents a single hardware lock::
 	* struct hwspinlock - this struct represents a single hwspinlock instance
 	* @bank: the hwspinlock_device structure which owns this lock
 	* @lock: initialized and used by hwspinlock core
+	* @refcount: number of users (when shared)
 	* @priv: private data, owned by the underlying platform-specific hwspinlock drv
 	*/
 	struct hwspinlock {
 		struct hwspinlock_device *bank;
 		spinlock_t lock;
+		unsigned int refcount;
 		void *priv;
 	};
 
diff --git a/drivers/hwspinlock/hwspinlock_core.c b/drivers/hwspinlock/hwspinlock_core.c
index fd5f5c5a5244..a2197af8973d 100644
--- a/drivers/hwspinlock/hwspinlock_core.c
+++ b/drivers/hwspinlock/hwspinlock_core.c
@@ -29,6 +29,8 @@
 
 /* radix tree tags */
 #define HWSPINLOCK_UNUSED	(0) /* tags an hwspinlock as unused */
+#define HWSPINLOCK_EXCLUSIVE	(1) /* tags an hwspinlock as exclusive */
+#define HWSPINLOCK_SHARED	(2) /* tags an hwspinlock as shared */
 
 /*
  * A radix tree is used to maintain the available hwspinlock instances.
@@ -308,7 +310,7 @@ EXPORT_SYMBOL_GPL(__hwspin_unlock);
  * @hwlock_spec: hwlock specifier as found in the device tree
  *
  * This is a simple translation function, suitable for hwspinlock platform
- * drivers that only has a lock specifier length of 1.
+ * drivers that only has a lock specifier length of 1 or 2.
  *
  * Returns a relative index of the lock within a specified bank on success,
  * or -EINVAL on invalid specifier cell count.
@@ -316,7 +318,8 @@ EXPORT_SYMBOL_GPL(__hwspin_unlock);
 static inline int
 of_hwspin_lock_simple_xlate(const struct of_phandle_args *hwlock_spec)
 {
-	if (WARN_ON(hwlock_spec->args_count != 1))
+	if (WARN_ON(hwlock_spec->args_count != 1 &&
+		    hwlock_spec->args_count != 2))
 		return -EINVAL;
 
 	return hwlock_spec->args[0];
@@ -339,11 +342,12 @@ of_hwspin_lock_simple_xlate(const struct of_phandle_args *hwlock_spec)
 int of_hwspin_lock_get_id(struct device_node *np, int index)
 {
 	struct of_phandle_args args;
-	struct hwspinlock *hwlock;
+	struct hwspinlock *hwlock, *tmp;
 	struct radix_tree_iter iter;
 	void **slot;
 	int id;
 	int ret;
+	unsigned int tag;
 
 	ret = of_parse_phandle_with_args(np, "hwlocks", "#hwlock-cells", index,
 					 &args);
@@ -383,6 +387,37 @@ int of_hwspin_lock_get_id(struct device_node *np, int index)
 	}
 	id += hwlock->bank->base_id;
 
+	/* Set the EXCLUSIVE / SHARED tag */
+	if (args.args_count == 2 && args.args[1]) {
+		/* Tag SHARED unless already tagged EXCLUSIVE */
+		if (radix_tree_tag_get(&hwspinlock_tree, id,
+				       HWSPINLOCK_EXCLUSIVE)) {
+			ret = -EINVAL;
+			goto out;
+		}
+		tag = HWSPINLOCK_SHARED;
+	} else {
+		/* Tag EXCLUSIVE unless already tagged SHARED */
+		if (radix_tree_tag_get(&hwspinlock_tree, id,
+				       HWSPINLOCK_SHARED)) {
+			ret = -EINVAL;
+			goto out;
+		}
+		tag = HWSPINLOCK_EXCLUSIVE;
+	}
+
+	/* mark this hwspinlock */
+	hwlock = radix_tree_lookup(&hwspinlock_tree, id);
+	if (!hwlock) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	tmp = radix_tree_tag_set(&hwspinlock_tree, id, tag);
+
+	/* self-sanity check which should never fail */
+	WARN_ON(tmp != hwlock);
+
 out:
 	of_node_put(args.np);
 	return ret ? ret : id;
@@ -505,6 +540,7 @@ int hwspin_lock_register(struct hwspinlock_device *bank, struct device *dev,
 
 		spin_lock_init(&hwlock->lock);
 		hwlock->bank = bank;
+		hwlock->refcount = 0;
 
 		ret = hwspin_lock_register_single(hwlock, base_id + i);
 		if (ret)
@@ -647,7 +683,7 @@ static int __hwspin_lock_request(struct hwspinlock *hwlock)
 {
 	struct device *dev = hwlock->bank->dev;
 	struct hwspinlock *tmp;
-	int ret;
+	int ret, id;
 
 	/* prevent underlying implementation from being removed */
 	if (!try_module_get(dev->driver->owner)) {
@@ -666,13 +702,18 @@ static int __hwspin_lock_request(struct hwspinlock *hwlock)
 
 	ret = 0;
 
+	/* update shareable refcount */
+	id = hwlock_to_id(hwlock);
+	if (radix_tree_tag_get(&hwspinlock_tree, id, HWSPINLOCK_SHARED) &&
+	    hwlock->refcount++)
+		goto out;
+
 	/* mark hwspinlock as used, should not fail */
-	tmp = radix_tree_tag_clear(&hwspinlock_tree, hwlock_to_id(hwlock),
-							HWSPINLOCK_UNUSED);
+	tmp = radix_tree_tag_clear(&hwspinlock_tree, id, HWSPINLOCK_UNUSED);
 
 	/* self-sanity check that should never fail */
 	WARN_ON(tmp != hwlock);
-
+out:
 	return ret;
 }
 
@@ -766,9 +807,9 @@ struct hwspinlock *hwspin_lock_request_specific(unsigned int id)
 	/* sanity check (this shouldn't happen) */
 	WARN_ON(hwlock_to_id(hwlock) != id);
 
-	/* make sure this hwspinlock is unused */
-	ret = radix_tree_tag_get(&hwspinlock_tree, id, HWSPINLOCK_UNUSED);
-	if (ret == 0) {
+	/* make sure this hwspinlock is unused or shareable */
+	if (!radix_tree_tag_get(&hwspinlock_tree, id, HWSPINLOCK_SHARED) &&
+	    !radix_tree_tag_get(&hwspinlock_tree, id, HWSPINLOCK_UNUSED)) {
 		pr_warn("hwspinlock %u is already in use\n", id);
 		hwlock = NULL;
 		goto out;
@@ -801,7 +842,7 @@ int hwspin_lock_free(struct hwspinlock *hwlock)
 {
 	struct device *dev;
 	struct hwspinlock *tmp;
-	int ret;
+	int ret, id;
 
 	if (!hwlock) {
 		pr_err("invalid hwlock\n");
@@ -812,28 +853,33 @@ int hwspin_lock_free(struct hwspinlock *hwlock)
 	mutex_lock(&hwspinlock_tree_lock);
 
 	/* make sure the hwspinlock is used */
-	ret = radix_tree_tag_get(&hwspinlock_tree, hwlock_to_id(hwlock),
-							HWSPINLOCK_UNUSED);
+	id = hwlock_to_id(hwlock);
+	ret = radix_tree_tag_get(&hwspinlock_tree, id, HWSPINLOCK_UNUSED);
 	if (ret == 1) {
 		dev_err(dev, "%s: hwlock is already free\n", __func__);
 		dump_stack();
 		ret = -EINVAL;
-		goto out;
+		goto unlock;
 	}
 
 	/* notify the underlying device that power is not needed */
 	pm_runtime_put(dev);
 
+	/* update shareable refcount */
+	if (radix_tree_tag_get(&hwspinlock_tree, id, HWSPINLOCK_SHARED) &&
+	    --hwlock->refcount)
+		goto put;
+
 	/* mark this hwspinlock as available */
-	tmp = radix_tree_tag_set(&hwspinlock_tree, hwlock_to_id(hwlock),
-							HWSPINLOCK_UNUSED);
+	tmp = radix_tree_tag_set(&hwspinlock_tree, id, HWSPINLOCK_UNUSED);
 
 	/* sanity check (this shouldn't happen) */
 	WARN_ON(tmp != hwlock);
 
+put:
 	module_put(dev->driver->owner);
 
-out:
+unlock:
 	mutex_unlock(&hwspinlock_tree_lock);
 	return ret;
 }
diff --git a/drivers/hwspinlock/hwspinlock_internal.h b/drivers/hwspinlock/hwspinlock_internal.h
index 29892767bb7a..e1f9c9600635 100644
--- a/drivers/hwspinlock/hwspinlock_internal.h
+++ b/drivers/hwspinlock/hwspinlock_internal.h
@@ -35,11 +35,13 @@ struct hwspinlock_ops {
  * struct hwspinlock - this struct represents a single hwspinlock instance
  * @bank: the hwspinlock_device structure which owns this lock
  * @lock: initialized and used by hwspinlock core
+ * @refcount: number of users (when shared)
  * @priv: private data, owned by the underlying platform-specific hwspinlock drv
  */
 struct hwspinlock {
 	struct hwspinlock_device *bank;
 	spinlock_t lock;
+	unsigned int refcount;
 	void *priv;
 };
 
diff --git a/drivers/hwspinlock/stm32_hwspinlock.c b/drivers/hwspinlock/stm32_hwspinlock.c
index 3ad0ce0da4d9..6c3be33f3faf 100644
--- a/drivers/hwspinlock/stm32_hwspinlock.c
+++ b/drivers/hwspinlock/stm32_hwspinlock.c
@@ -54,8 +54,23 @@ static const struct hwspinlock_ops stm32_hwspinlock_ops = {
 	.relax		= stm32_hwspinlock_relax,
 };
 
+static void stm32_hwspinlock_disable_clk(void *data)
+{
+	struct platform_device *pdev = data;
+	struct stm32_hwspinlock *hw = platform_get_drvdata(pdev);
+	struct device *dev = &pdev->dev;
+
+	pm_runtime_get_sync(dev);
+	pm_runtime_disable(dev);
+	pm_runtime_set_suspended(dev);
+	pm_runtime_put_noidle(dev);
+
+	clk_disable_unprepare(hw->clk);
+}
+
 static int stm32_hwspinlock_probe(struct platform_device *pdev)
 {
+	struct device *dev = &pdev->dev;
 	struct stm32_hwspinlock *hw;
 	void __iomem *io_base;
 	size_t array_size;
@@ -66,41 +81,43 @@ static int stm32_hwspinlock_probe(struct platform_device *pdev)
 		return PTR_ERR(io_base);
 
 	array_size = STM32_MUTEX_NUM_LOCKS * sizeof(struct hwspinlock);
-	hw = devm_kzalloc(&pdev->dev, sizeof(*hw) + array_size, GFP_KERNEL);
+	hw = devm_kzalloc(dev, sizeof(*hw) + array_size, GFP_KERNEL);
 	if (!hw)
 		return -ENOMEM;
 
-	hw->clk = devm_clk_get(&pdev->dev, "hsem");
+	hw->clk = devm_clk_get(dev, "hsem");
 	if (IS_ERR(hw->clk))
 		return PTR_ERR(hw->clk);
 
-	for (i = 0; i < STM32_MUTEX_NUM_LOCKS; i++)
-		hw->bank.lock[i].priv = io_base + i * sizeof(u32);
+	ret = clk_prepare_enable(hw->clk);
+	if (ret) {
+		dev_err(dev, "Failed to prepare_enable clock\n");
+		return ret;
+	}
+
+	pm_runtime_get_noresume(dev);
+	pm_runtime_set_active(dev);
+	pm_runtime_enable(dev);
+	pm_runtime_put(dev);
 
 	platform_set_drvdata(pdev, hw);
-	pm_runtime_enable(&pdev->dev);
 
-	ret = hwspin_lock_register(&hw->bank, &pdev->dev, &stm32_hwspinlock_ops,
-				   0, STM32_MUTEX_NUM_LOCKS);
+	ret = devm_add_action_or_reset(dev, stm32_hwspinlock_disable_clk, pdev);
+	if (ret) {
+		dev_err(dev, "Failed to register action\n");
+		return ret;
+	}
 
-	if (ret)
-		pm_runtime_disable(&pdev->dev);
-
-	return ret;
-}
+	for (i = 0; i < STM32_MUTEX_NUM_LOCKS; i++)
+		hw->bank.lock[i].priv = io_base + i * sizeof(u32);
 
-static int stm32_hwspinlock_remove(struct platform_device *pdev)
-{
-	struct stm32_hwspinlock *hw = platform_get_drvdata(pdev);
-	int ret;
+	ret = devm_hwspin_lock_register(dev, &hw->bank, &stm32_hwspinlock_ops,
+					0, STM32_MUTEX_NUM_LOCKS);
 
-	ret = hwspin_lock_unregister(&hw->bank);
 	if (ret)
-		dev_err(&pdev->dev, "%s failed: %d\n", __func__, ret);
-
-	pm_runtime_disable(&pdev->dev);
+		dev_err(dev, "Failed to register hwspinlock\n");
 
-	return 0;
+	return ret;
 }
 
 static int __maybe_unused stm32_hwspinlock_runtime_suspend(struct device *dev)
@@ -135,7 +152,6 @@ MODULE_DEVICE_TABLE(of, stm32_hwpinlock_ids);
 
 static struct platform_driver stm32_hwspinlock_driver = {
 	.probe		= stm32_hwspinlock_probe,
-	.remove		= stm32_hwspinlock_remove,
 	.driver		= {
 		.name	= "stm32_hwspinlock",
 		.of_match_table = stm32_hwpinlock_ids,
-- 
2.17.1

