[PATCH 4/4] 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 | 70 +++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 71 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..6ad75eb113
--- /dev/null
+++ b/security/kernel_pinctrl.c
@@ -0,0 +1,70 @@
+// 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)
+{
+	struct device_node *n;
+	const char *names[NUM_NAMES];
+	int num_read = of_property_read_string_array(node, "pinctrl-names", names, NUM_NAMES);
+	struct property *prop = of_find_property(node, "pinctrl-names", NULL);
+	int 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) {
+		// old_default is shorter then barebox,policy-*
+		char *val = malloc(prop->length);
+		char *cur = val;
+		const char *src;
+		int len;
+
+		for (int i = 0; i < num_read; i++) {
+			if (i == pos_policy)
+				src = "default";
+			else if (i == pos_default)
+				src = "old_default";
+			else
+				src = names[i];
+			len = strlen(src);
+			memcpy(cur, src, len);
+			cur += len;
+		}
+		of_set_property(node, "pinctrl-names", val, cur - val, false);
+	}
+
+	list_for_each_entry(n, &node->children, parent_list) {
+		kernel_of_fixup_pinctrl(n, policy_name);
+	}
+}
+
+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