[PATCH v2 5/5] security: kernel_pinctrl: fixup pinctrl in kernel dts

[Fabian Pflug <f.pflug@pengutronix.de>]

Going through the kernel dts and replacing
barebox,policy-<active_policy> with default in order to change pinctrl
not only for barebox, but also for kernel when booting with security
profiles.

Signed-off-by: Fabian Pflug <f.pflug@pengutronix.de>
---
 security/Makefile         |  1 +
 security/kernel_pinctrl.c | 58 +++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 59 insertions(+)

diff --git a/security/Makefile b/security/Makefile
index 1096cbfb9b..2e8cdfe7c2 100644
--- a/security/Makefile
+++ b/security/Makefile
@@ -2,6 +2,7 @@
 
 obj-$(CONFIG_SECURITY_POLICY)		+= policy.o
 obj-$(CONFIG_SECURITY_POLICY_NAMES)	+= sconfig_names.o
+obj-$(CONFIG_SECURITY_POLICY_PINCTRL)	+= kernel_pinctrl.o
 obj-$(CONFIG_CRYPTO_KEYSTORE)	+= keystore.o
 obj-$(CONFIG_JWT)		+= jwt.o
 obj-pbl-$(CONFIG_HAVE_OPTEE)	+= optee.o
diff --git a/security/kernel_pinctrl.c b/security/kernel_pinctrl.c
new file mode 100644
index 0000000000..af65961bbc
--- /dev/null
+++ b/security/kernel_pinctrl.c
@@ -0,0 +1,58 @@
+// SPDX-License-Identifier: GPL-2.0-only
+
+#include <common.h>
+#include <linux/printk.h>
+#include <pinctrl.h>
+#include <security/policy.h>
+#include <security/config.h>
+
+#define NUM_NAMES	10
+
+
+/**
+ * Replace 'default' with 'old_default' and 'barebox,policy-<active_policy>'
+ * with 'default', if both are found in pinctrl.
+ */
+static void kernel_of_fixup_pinctrl(struct device_node *node, char *policy_name)
+{
+	const char *names[NUM_NAMES];
+	int num_read, pos_default, pos_policy;
+	struct device_node *next;
+
+	while (node) {
+		num_read = of_property_read_string_array(node, "pinctrl-names", names, NUM_NAMES);
+
+		pos_default = -1;
+		pos_policy = -1;
+		for (int i = 0; i < num_read; i++) {
+			if (strcmp(policy_name, names[i]) == 0)
+				pos_policy = i;
+			if (strcmp("default", names[i]) == 0)
+				pos_default = i;
+		}
+		if (pos_default >= 0 && pos_policy >= 0) {
+			names[pos_default] = "old_default";
+			names[pos_policy] = "default";
+			of_property_write_string_array(node, "pinctrl-names", names, num_read);
+		}
+
+		next = list_first_entry(&node->list, struct device_node, list);
+		node = next->parent ? next : NULL;
+	}
+}
+
+static int kernel_of_fixup_pinctrl_start(struct device_node *root, void *unused)
+{
+	char *policy_pinctrl;
+
+	policy_pinctrl = basprintf("barebox,policy-%s", active_policy->name);
+	kernel_of_fixup_pinctrl(root, policy_pinctrl);
+	free(policy_pinctrl);
+	return 0;
+}
+
+static int policy_console_pinctrl_init(void)
+{
+	return of_register_fixup(kernel_of_fixup_pinctrl_start, NULL);
+}
+late_initcall(policy_console_pinctrl_init);

-- 
2.47.3