[PATCH v2 02/11] firmware: qcom: scm: add a dedicated SCM memory allocator

[Bartosz Golaszewski <brgl@bgdev.pl>]

From: Bartosz Golaszewski <bartosz.golaszewski@linaro.org>

We have several SCM calls that require passing buffers to the trustzone
on top of the SMC core which allocated memory for calls that require
more than 4 arguments.

Currently every user does their own thing which leads to code
duplication. Many users call dma_alloc_coherent() for every call which
is terribly unperformant (speed- and size-wise).

As all but one calls allocate memory just for the duration of the call,
we don't need a lot of memory. A single pool for that purpose is enough.
Let's create a genalloc pool dealing out chunks of coherent, page-aligned
memory suitable for SCM calls that also provides a function for mapping
virtual to physical addresses.

Signed-off-by: Bartosz Golaszewski <bartosz.golaszewski@linaro.org>
---
 drivers/firmware/qcom/Makefile         |   2 +-
 drivers/firmware/qcom/qcom_scm-mem.c   | 134 +++++++++++++++++++++++++
 drivers/firmware/qcom/qcom_scm.c       |   5 +
 drivers/firmware/qcom/qcom_scm.h       |   7 ++
 include/linux/firmware/qcom/qcom_scm.h |   7 ++
 5 files changed, 154 insertions(+), 1 deletion(-)
 create mode 100644 drivers/firmware/qcom/qcom_scm-mem.c

diff --git a/drivers/firmware/qcom/Makefile b/drivers/firmware/qcom/Makefile
index c9f12ee8224a..b9b117f22e9f 100644
--- a/drivers/firmware/qcom/Makefile
+++ b/drivers/firmware/qcom/Makefile
@@ -4,6 +4,6 @@
 #
 
 obj-$(CONFIG_QCOM_SCM)		+= qcom-scm.o
-qcom-scm-objs += qcom_scm.o qcom_scm-smc.o qcom_scm-legacy.o
+qcom-scm-objs += qcom_scm.o qcom_scm-smc.o qcom_scm-legacy.o qcom_scm-mem.o
 obj-$(CONFIG_QCOM_QSEECOM)	+= qcom_qseecom.o
 obj-$(CONFIG_QCOM_QSEECOM_UEFISECAPP) += qcom_qseecom_uefisecapp.o
diff --git a/drivers/firmware/qcom/qcom_scm-mem.c b/drivers/firmware/qcom/qcom_scm-mem.c
new file mode 100644
index 000000000000..eafecbe23770
--- /dev/null
+++ b/drivers/firmware/qcom/qcom_scm-mem.c
@@ -0,0 +1,134 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2023 Linaro Ltd.
+ */
+
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/firmware/qcom/qcom_scm.h>
+#include <linux/genalloc.h>
+#include <linux/gfp.h>
+#include <linux/moduleparam.h>
+#include <linux/radix-tree.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/types.h>
+
+#include "qcom_scm.h"
+
+static size_t qcom_scm_mem_pool_size = SZ_2M;
+module_param_named(qcom_scm_mem_pool_size, qcom_scm_mem_pool_size,
+		   ulong, 0400);
+
+struct {
+	struct device *dev;
+	void *vbase;
+	phys_addr_t pbase;
+	size_t size;
+	struct gen_pool *pool;
+	struct radix_tree_root chunks;
+	spinlock_t lock;
+} qcom_scm_mem;
+
+struct qcom_scm_mem_chunk {
+	phys_addr_t paddr;
+	size_t size;
+};
+
+void *qcom_scm_mem_alloc(size_t size, gfp_t gfp)
+{
+	struct qcom_scm_mem_chunk *chunk;
+	unsigned long vaddr;
+	int ret;
+
+	if (!size)
+		return ZERO_SIZE_PTR;
+
+	size = roundup(size, 1 << PAGE_SHIFT);
+
+	chunk = kzalloc(sizeof(*chunk), gfp);
+	if (!chunk)
+		return NULL;
+
+	vaddr = gen_pool_alloc(qcom_scm_mem.pool, size);
+	if (!vaddr) {
+		kfree(chunk);
+		return NULL;
+	}
+
+	chunk->paddr = gen_pool_virt_to_phys(qcom_scm_mem.pool,
+					     (unsigned long)vaddr);
+	chunk->size = size;
+
+	scoped_guard(spinlock_irqsave, &qcom_scm_mem.lock) {
+		ret = radix_tree_insert(&qcom_scm_mem.chunks, vaddr, chunk);
+		if (ret) {
+			gen_pool_free(qcom_scm_mem.pool, (unsigned long)vaddr,
+				      chunk->size);
+			kfree(chunk);
+			return NULL;
+		}
+	}
+
+	return (void *)vaddr;
+}
+EXPORT_SYMBOL_GPL(qcom_scm_mem_alloc);
+
+void qcom_scm_mem_free(void *vaddr)
+{
+	struct qcom_scm_mem_chunk *chunk;
+
+	if (!vaddr)
+		return;
+
+	scoped_guard(spinlock_irqsave, &qcom_scm_mem.lock)
+		chunk = radix_tree_delete_item(&qcom_scm_mem.chunks,
+					       (unsigned long)vaddr, NULL);
+
+	if (!chunk) {
+		WARN(1, "Virtual address %p not allocated for SCM", vaddr);
+		return;
+	}
+
+	gen_pool_free(qcom_scm_mem.pool, (unsigned long)vaddr, chunk->size);
+	kfree(chunk);
+}
+EXPORT_SYMBOL_GPL(qcom_scm_mem_free);
+
+phys_addr_t qcom_scm_mem_to_phys(void *vaddr)
+{
+	struct qcom_scm_mem_chunk *chunk;
+
+	guard(spinlock_irqsave)(&qcom_scm_mem.lock);
+
+	chunk = radix_tree_lookup(&qcom_scm_mem.chunks, (unsigned long)vaddr);
+	if (!chunk)
+		return 0;
+
+	return chunk->paddr;
+}
+
+int qcom_scm_mem_enable(struct device *dev)
+{
+	INIT_RADIX_TREE(&qcom_scm_mem.chunks, GFP_ATOMIC);
+	spin_lock_init(&qcom_scm_mem.lock);
+	qcom_scm_mem.dev = dev;
+	qcom_scm_mem.size = qcom_scm_mem_pool_size;
+
+	qcom_scm_mem.vbase = dmam_alloc_coherent(dev, qcom_scm_mem.size,
+						 &qcom_scm_mem.pbase,
+						 GFP_KERNEL);
+	if (!qcom_scm_mem.vbase)
+		return -ENOMEM;
+
+	qcom_scm_mem.pool = devm_gen_pool_create(dev, PAGE_SHIFT, -1,
+						 "qcom-scm-mem");
+	if (!qcom_scm_mem.pool)
+		return -ENOMEM;
+
+	gen_pool_set_algo(qcom_scm_mem.pool, gen_pool_best_fit, NULL);
+
+	return gen_pool_add_virt(qcom_scm_mem.pool,
+				 (unsigned long)qcom_scm_mem.vbase,
+				 qcom_scm_mem.pbase, qcom_scm_mem.size, -1);
+}
diff --git a/drivers/firmware/qcom/qcom_scm.c b/drivers/firmware/qcom/qcom_scm.c
index c2c7fafef34b..258aa0782754 100644
--- a/drivers/firmware/qcom/qcom_scm.c
+++ b/drivers/firmware/qcom/qcom_scm.c
@@ -1880,6 +1880,11 @@ static int qcom_scm_probe(struct platform_device *pdev)
 	if (of_property_read_bool(pdev->dev.of_node, "qcom,sdi-enabled"))
 		qcom_scm_disable_sdi();
 
+	ret = qcom_scm_mem_enable(scm->dev);
+	if (ret)
+		return dev_err_probe(scm->dev, ret,
+				     "Failed to enable SCM memory\n");
+
 	/*
 	 * Initialize the QSEECOM interface.
 	 *
diff --git a/drivers/firmware/qcom/qcom_scm.h b/drivers/firmware/qcom/qcom_scm.h
index 7b68fa820495..8c97e3906afa 100644
--- a/drivers/firmware/qcom/qcom_scm.h
+++ b/drivers/firmware/qcom/qcom_scm.h
@@ -4,6 +4,10 @@
 #ifndef __QCOM_SCM_INT_H
 #define __QCOM_SCM_INT_H
 
+#include <linux/types.h>
+
+struct device;
+
 enum qcom_scm_convention {
 	SMC_CONVENTION_UNKNOWN,
 	SMC_CONVENTION_LEGACY,
@@ -165,4 +169,7 @@ static inline int qcom_scm_remap_error(int err)
 	return -EINVAL;
 }
 
+int qcom_scm_mem_enable(struct device *dev);
+phys_addr_t qcom_scm_mem_to_phys(void *vaddr);
+
 #endif
diff --git a/include/linux/firmware/qcom/qcom_scm.h b/include/linux/firmware/qcom/qcom_scm.h
index ccaf28846054..291ef8fd21b0 100644
--- a/include/linux/firmware/qcom/qcom_scm.h
+++ b/include/linux/firmware/qcom/qcom_scm.h
@@ -5,7 +5,9 @@
 #ifndef __QCOM_SCM_H
 #define __QCOM_SCM_H
 
+#include <linux/cleanup.h>
 #include <linux/err.h>
+#include <linux/gfp.h>
 #include <linux/types.h>
 #include <linux/cpumask.h>
 
@@ -61,6 +63,11 @@ enum qcom_scm_ice_cipher {
 
 bool qcom_scm_is_available(void);
 
+void *qcom_scm_mem_alloc(size_t size, gfp_t gfp);
+void qcom_scm_mem_free(void *vaddr);
+
+DEFINE_FREE(qcom_scm_mem, void *, if (_T) qcom_scm_mem_free(_T));
+
 int qcom_scm_set_cold_boot_addr(void *entry);
 int qcom_scm_set_warm_boot_addr(void *entry);
 void qcom_scm_cpu_power_down(u32 flags);
-- 
2.39.2