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

Bartosz Golaszewski
2023-09-28T09:21:31Z

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