139 lines
3.4 KiB
Diff
139 lines
3.4 KiB
Diff
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
|
|
From: =?UTF-8?q?Arve=20Hj=C3=B8nnev=C3=A5g?= <arve@android.com>
|
|
Date: Fri, 2 May 2014 19:15:44 -0700
|
|
Subject: [PATCH] trusty: fiq-arm64: Allow multiple fiq handlers.
|
|
MIME-Version: 1.0
|
|
Content-Type: text/plain; charset=UTF-8
|
|
Content-Transfer-Encoding: 8bit
|
|
|
|
If multiple fiq handlers are reqistered call them all. There is
|
|
currently no api to remove handlers.
|
|
|
|
Change-Id: I1d4bd936081d690ea6f1ec0c041f43a5f7717733
|
|
Signed-off-by: Arve Hjønnevåg <arve@android.com>
|
|
---
|
|
drivers/trusty/trusty-fiq-arm64.c | 76 ++++++++++++++++++++++---------
|
|
1 file changed, 54 insertions(+), 22 deletions(-)
|
|
|
|
diff --git a/drivers/trusty/trusty-fiq-arm64.c b/drivers/trusty/trusty-fiq-arm64.c
|
|
index df05a98f235d..8b9a40887587 100644
|
|
--- a/drivers/trusty/trusty-fiq-arm64.c
|
|
+++ b/drivers/trusty/trusty-fiq-arm64.c
|
|
@@ -26,12 +26,19 @@ extern void trusty_fiq_glue_arm64(void);
|
|
|
|
static struct device *trusty_dev;
|
|
static DEFINE_PER_CPU(void *, fiq_stack);
|
|
-static struct fiq_glue_handler *current_handler;
|
|
+static struct fiq_glue_handler *fiq_handlers;
|
|
static DEFINE_MUTEX(fiq_glue_lock);
|
|
|
|
void trusty_fiq_handler(struct pt_regs *regs, void *svc_sp)
|
|
{
|
|
- current_handler->fiq(current_handler, regs, svc_sp);
|
|
+ struct fiq_glue_handler *handler;
|
|
+
|
|
+ for (handler = ACCESS_ONCE(fiq_handlers); handler;
|
|
+ handler = ACCESS_ONCE(handler->next)) {
|
|
+ /* Barrier paired with smp_wmb in fiq_glue_register_handler */
|
|
+ smp_read_barrier_depends();
|
|
+ handler->fiq(handler, regs, svc_sp);
|
|
+ }
|
|
}
|
|
|
|
static void smp_nop_call(void *info)
|
|
@@ -64,29 +71,13 @@ static void fiq_glue_clear_handler(void)
|
|
}
|
|
}
|
|
|
|
-int fiq_glue_register_handler(struct fiq_glue_handler *handler)
|
|
+static int fiq_glue_set_handler(void)
|
|
{
|
|
int ret;
|
|
int cpu;
|
|
void *stack;
|
|
unsigned long irqflags;
|
|
|
|
- if (!handler || !handler->fiq)
|
|
- return -EINVAL;
|
|
-
|
|
- mutex_lock(&fiq_glue_lock);
|
|
-
|
|
- if (!trusty_dev) {
|
|
- ret = -ENODEV;
|
|
- goto err_no_trusty;
|
|
- }
|
|
- if (current_handler) {
|
|
- ret = -EBUSY;
|
|
- goto err_busy;
|
|
- }
|
|
-
|
|
- current_handler = handler;
|
|
-
|
|
for_each_possible_cpu(cpu) {
|
|
stack = (void *)__get_free_pages(GFP_KERNEL, THREAD_SIZE_ORDER);
|
|
if (WARN_ON(!stack)) {
|
|
@@ -109,16 +100,57 @@ int fiq_glue_register_handler(struct fiq_glue_handler *handler)
|
|
goto err_set_fiq_handler;
|
|
}
|
|
}
|
|
+ return 0;
|
|
+
|
|
+err_alloc_fiq_stack:
|
|
+err_set_fiq_handler:
|
|
+ fiq_glue_clear_handler();
|
|
+ return ret;
|
|
+}
|
|
+
|
|
+int fiq_glue_register_handler(struct fiq_glue_handler *handler)
|
|
+{
|
|
+ int ret;
|
|
+
|
|
+ if (!handler || !handler->fiq) {
|
|
+ ret = -EINVAL;
|
|
+ goto err_bad_arg;
|
|
+ }
|
|
+
|
|
+ mutex_lock(&fiq_glue_lock);
|
|
+
|
|
+ if (!trusty_dev) {
|
|
+ ret = -ENODEV;
|
|
+ goto err_no_trusty;
|
|
+ }
|
|
+
|
|
+ handler->next = fiq_handlers;
|
|
+ /*
|
|
+ * Write barrier paired with smp_read_barrier_depends in
|
|
+ * trusty_fiq_handler. Make sure next pointer is updated before
|
|
+ * fiq_handlers so trusty_fiq_handler does not see an uninitialized
|
|
+ * value and terminate early or crash.
|
|
+ */
|
|
+ smp_wmb();
|
|
+ fiq_handlers = handler;
|
|
+
|
|
+ smp_call_function(smp_nop_call, NULL, true);
|
|
+
|
|
+ if (!handler->next) {
|
|
+ ret = fiq_glue_set_handler();
|
|
+ if (ret)
|
|
+ goto err_set_fiq_handler;
|
|
+ }
|
|
|
|
mutex_unlock(&fiq_glue_lock);
|
|
return 0;
|
|
|
|
err_set_fiq_handler:
|
|
-err_alloc_fiq_stack:
|
|
- fiq_glue_clear_handler();
|
|
-err_busy:
|
|
+ fiq_handlers = handler->next;
|
|
err_no_trusty:
|
|
mutex_unlock(&fiq_glue_lock);
|
|
+err_bad_arg:
|
|
+ pr_err("%s: failed, %d\n", __func__, ret);
|
|
return ret;
|
|
}
|
|
|
|
--
|
|
https://clearlinux.org
|
|
|