core: framework to register PM callbacks

Introduce a framework for power management callback registering.

Drivers and services can register a callback function for the platform
suspend and resume sequences. A private address handle can be registered
with the callback and retrieved from the callback. Callback can be
registered with a specific call order as defined per PM_CB_ORDER_*.

Callback shall return an error if failing to complete target transition.
This information may be used by the platform to resume a platform on
non-fatal failure to suspend.

Callbacks are related to a callback level. It defines the callbacks
call ordering, allowing core low level drivers (as clocks or the GIC)
to be suspended after all drivers and resume before these.

Signed-off-by: Etienne Carriere <etienne.carriere@linaro.org>
Reviewed-by: Jens Wiklander <jens.wiklander@linaro.org>
Acked-by: Cedric Neveux <cedric.neveux@nxp.com>

2 files changed, 98 insertions, 0 deletions

diff --git a/core/kernel/pm.c b/core/kernel/pm.c
new file mode 100644
index 00000000..b51d73f4
--- /dev/null
+++ b/core/kernel/pm.c
@@ -0,0 +1,97 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (c) 2018, Linaro Limited
+ */
+
+#include <keep.h>
+#include <kernel/panic.h>
+#include <kernel/pm.h>
+#include <mm/core_memprot.h>
+#include <string.h>
+#include <types_ext.h>
+
+#define PM_FLAG_SUSPENDED	BIT(0)
+
+static struct pm_callback_handle *pm_cb_ref;
+static size_t pm_cb_count;
+
+static void verify_cb_args(struct pm_callback_handle *pm_hdl)
+{
+	if (is_unpaged((void *)(vaddr_t)pm_change_state) &&
+	    (!is_unpaged((void *)(vaddr_t)pm_hdl->callback) ||
+	     (pm_hdl->handle && !is_unpaged(pm_hdl->handle)))) {
+		EMSG("PM callbacks mandates unpaged arguments: %p %p",
+		     (void *)(vaddr_t)pm_hdl->callback, pm_hdl->handle);
+		panic();
+	}
+}
+
+void register_pm_cb(struct pm_callback_handle *pm_hdl)
+{
+	size_t count = pm_cb_count;
+	struct pm_callback_handle *ref;
+
+	verify_cb_args(pm_hdl);
+
+	ref = realloc(pm_cb_ref, sizeof(*ref) * (count + 1));
+	if (!ref)
+		panic();
+
+	ref[count] = *pm_hdl;
+	ref[count].flags = 0;
+
+	pm_cb_count = count + 1;
+	pm_cb_ref = ref;
+}
+
+static TEE_Result call_callbacks(enum pm_op op, uint32_t pm_hint,
+				 enum pm_callback_order order)
+{
+	struct pm_callback_handle *hdl;
+	size_t n;
+	TEE_Result res;
+
+	for (n = 0, hdl = pm_cb_ref; n < pm_cb_count; n++, hdl++) {
+		if (hdl->order != order ||
+		    (hdl->flags & PM_FLAG_SUSPENDED) == (op == PM_OP_SUSPEND))
+			continue;
+
+		res = hdl->callback(op, pm_hint, hdl);
+		if (res)
+			return res;
+
+		if (op == PM_OP_SUSPEND)
+			hdl->flags |= PM_FLAG_SUSPENDED;
+		else
+			hdl->flags &= ~PM_FLAG_SUSPENDED;
+	}
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result pm_change_state(enum pm_op op, uint32_t pm_hint)
+{
+	enum pm_callback_order cnt;
+	TEE_Result res;
+
+	switch (op) {
+	case PM_OP_SUSPEND:
+		for (cnt = PM_CB_ORDER_DRIVER; cnt < PM_CB_ORDER_MAX; cnt++) {
+			res = call_callbacks(op, pm_hint, cnt);
+			if (res)
+				return res;
+		}
+		break;
+	case PM_OP_RESUME:
+		for (cnt = PM_CB_ORDER_MAX; cnt > PM_CB_ORDER_DRIVER; cnt--) {
+			res = call_callbacks(op, pm_hint, cnt - 1);
+			if (res)
+				return res;
+		}
+		break;
+	default:
+		panic();
+	}
+
+	return TEE_SUCCESS;
+}
diff --git a/core/kernel/sub.mk b/core/kernel/sub.mk
index e6382fa4..40af7855 100644
--- a/core/kernel/sub.mk
+++ b/core/kernel/sub.mk
@@ -3,6 +3,7 @@ cflags-remove-asan.c-y += $(cflags_kasan)
 srcs-y += assert.c
 srcs-y += console.c
 srcs-$(CFG_DT) += dt.c
+srcs-y += pm.c
 srcs-y += handle.c
 srcs-y += interrupt.c
 srcs-$(CFG_LOCKDEP) += lockdep.c