Message-Id: <20241021150112.1194-1-rex.nie@jaguarmicro.com>
Date: Mon, 21 Oct 2024 23:01:12 +0800
From: Rex Nie <rex.nie@...uarmicro.com>
To: mcoquelin.stm32@...il.com,
	alexandre.torgue@...s.st.com
Cc: linux-kernel@...r.kernel.org,
	linux-stm32@...md-mailman.stormreply.com,
	linux-arm-kernel@...ts.infradead.org,
	angus.chen@...uarmicro.com,
	Rex Nie <rex.nie@...uarmicro.com>
Subject: [PATCH] tools: arm64: add registers read/write tool for arm64

The reg_ctrl kernel module can read/write most aarch64 system registers,
including EL0/1/2/3, which is very useful when hardware debuger (such
as ArmDS5/trace32) is unusable.

The primary implementation of the reg_ctrl module is as follows:
1. when the core can directly access the target register, it uses
   the MRS/MSR instructions to read/write register.
2. Otherwise, it performs an SMC call to switch to EL3, where the
   register read/write is completed and then return to kernel mode.
   I implement an OEM Service in ATF to access register at EL3,
   using one SMC function ID for reading and another for writing registers.

test steps on my platform with 16x Arm Neoverse N2:
1. insmod reg_ctrl.ko
2. cd /sys/kernel/reg_ctrl/system/
3. view the directory tree on DUT.
[root@...alhost system]# tree
.
├── control
│   └── VNCR_EL2
├── id
│   ├── CCSIDR_EL1
│   ├── CLIDR_EL1
│   ├── CSSELR_EL1
│   ├── CTR_EL0
│   ├── DCZID_EL0
│   ├── ID_AA64AFR0_EL1
│   ├── ID_AA64AFR1_EL1
│   ├── ID_AA64DFR0_EL1
│   ├── ID_AA64DFR1_EL1
│   ├── ID_AA64ISAR0_EL1
│   ├── ID_AA64ISAR1_EL1
│   ├── ID_AA64MMFR0_EL1
│   ├── ID_AA64MMFR1_EL1
│   ├── ID_AA64PFR0_EL1
│   └── ID_AA64PFR1_EL1
├── implementation_defined
│   ├── IMP_CPUACTLR_EL3
│   ├── IMP_CPUECTLR_EL1
│   ├── IMP_CPUPPMCR2_EL3
│   ├── IMP_CPUPPMCR4_EL3
│   ├── IMP_CPUPPMCR5_EL3
│   ├── IMP_CPUPPMCR6_EL3
│   └── IMP_CPUPPMCR_EL3
└── reset
    └── RMR_EL3

4. read EL1 register on core 0:
[root@...alhost system]# taskset -c 0 cat id/ID_AA64PFR0_EL1
0x1201111123111112

5. read EL3 register on core 1:
[root@...alhost system]# taskset -c 1 cat implementation_defined/IMP_CPUPPMCR4_EL3
0x2000315a10000045

6. set bit 1 of IMP_CPUPPMCR4_EL3 regiter on core 1:
[root@...alhost system]# taskset -c 1 echo 0x2000315a10000047 > implementation_defined/IMP_CPUPPMCR4_EL3

7. check if bit 1 is set:
[root@...alhost system]# taskset -c 1 cat implementation_defined/IMP_CPUPPMCR4_EL3
0x2000315a10000047

Signed-off-by: Rex Nie <rex.nie@...uarmicro.com>
---
 tools/arch/arm64/tools/reg_ctrl/Makefile      |   8 +
 tools/arch/arm64/tools/reg_ctrl/reg_ctrl.c    | 247 ++++++++++++++++++
 tools/arch/arm64/tools/reg_ctrl/reg_ctrl.h    |  98 +++++++
 tools/arch/arm64/tools/reg_ctrl/sysreg.h      |  20 ++
 tools/arch/arm64/tools/reg_ctrl/testme.sh     |  17 ++
 .../arm64/tools/reg_ctrl/testme_all_cpu.sh    |  15 ++
 6 files changed, 405 insertions(+)
 create mode 100644 tools/arch/arm64/tools/reg_ctrl/Makefile
 create mode 100644 tools/arch/arm64/tools/reg_ctrl/reg_ctrl.c
 create mode 100644 tools/arch/arm64/tools/reg_ctrl/reg_ctrl.h
 create mode 100644 tools/arch/arm64/tools/reg_ctrl/sysreg.h
 create mode 100644 tools/arch/arm64/tools/reg_ctrl/testme.sh
 create mode 100644 tools/arch/arm64/tools/reg_ctrl/testme_all_cpu.sh

diff --git a/tools/arch/arm64/tools/reg_ctrl/Makefile b/tools/arch/arm64/tools/reg_ctrl/Makefile
new file mode 100644
index 000000000000..a7ac0e6a013c
--- /dev/null
+++ b/tools/arch/arm64/tools/reg_ctrl/Makefile
@@ -0,0 +1,8 @@
+obj-m += reg_ctrl.o
+
+all:
+	make -C /lib/modules/$(shell uname -r)/build M=$(PWD) modules
+
+clean:
+	make -C /lib/modules/$(shell uname -r)/build M=$(PWD) clean
+
diff --git a/tools/arch/arm64/tools/reg_ctrl/reg_ctrl.c b/tools/arch/arm64/tools/reg_ctrl/reg_ctrl.c
new file mode 100644
index 000000000000..70984800ef08
--- /dev/null
+++ b/tools/arch/arm64/tools/reg_ctrl/reg_ctrl.c
@@ -0,0 +1,247 @@
+// SPDX-License-Identifier: GPL-2.0
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/kobject.h>
+#include <linux/sysfs.h>
+#include <asm/sysreg.h>
+#include <linux/string.h>
+
+#include "reg_ctrl.h"
+#include "sysreg.h"
+
+#define SMCCC_OEM_REG_CTRL_READ_REG 0xC3000002
+#define SMCCC_OEM_REG_CTRL_WRITE_REG 0xC3000003
+
+static struct kobject *reg_kobj;
+static struct kobject *system_kobj;
+
+static ssize_t reg_show(struct kobject *kobj, struct kobj_attribute *attr,
+		char *buf);
+
+static ssize_t reg_store(struct kobject *kobj, struct kobj_attribute *attr,
+		const char *buf, size_t count);
+
+FUNC_SYSREG_S_RW(VNCR_EL2, SYS_VNCR_EL2);
+
+FUNC_SYSREG_S_RO(CCSIDR_EL1, SYS_CCSIDR_EL1);
+FUNC_SYSREG_S_RO(CLIDR_EL1, SYS_CLIDR_EL1);
+FUNC_SYSREG_S_RO(CSSELR_EL1, SYS_CSSELR_EL1);
+FUNC_SYSREG_S_RO(CTR_EL0, SYS_CTR_EL0);
+FUNC_SYSREG_S_RO(DCZID_EL0, SYS_DCZID_EL0);
+FUNC_SYSREG_S_RO(ID_AA64AFR0_EL1, SYS_ID_AA64AFR0_EL1);
+FUNC_SYSREG_S_RO(ID_AA64AFR1_EL1, SYS_ID_AA64AFR1_EL1);
+FUNC_SYSREG_S_RO(ID_AA64DFR0_EL1, SYS_ID_AA64DFR0_EL1);
+FUNC_SYSREG_S_RO(ID_AA64DFR1_EL1, SYS_ID_AA64DFR1_EL1);
+FUNC_SYSREG_S_RO(ID_AA64ISAR0_EL1, SYS_ID_AA64ISAR0_EL1);
+FUNC_SYSREG_S_RO(ID_AA64ISAR1_EL1, SYS_ID_AA64ISAR1_EL1);
+FUNC_SYSREG_S_RO(ID_AA64MMFR0_EL1, SYS_ID_AA64MMFR0_EL1);
+FUNC_SYSREG_S_RO(ID_AA64MMFR1_EL1, SYS_ID_AA64MMFR1_EL1);
+FUNC_SYSREG_S_RO(ID_AA64PFR0_EL1, SYS_ID_AA64PFR0_EL1);
+FUNC_SYSREG_S_RO(ID_AA64PFR1_EL1, SYS_ID_AA64PFR1_EL1);
+
+FUNC_SYSREG_SMC_RW(RMR_EL3, SYS_RMR_EL3);
+
+FUNC_SYSREG_S_RW(IMP_CPUECTLR_EL1, SYS_IMP_CPUECTLR_EL1);
+FUNC_SYSREG_SMC_RW(IMP_CPUACTLR_EL3, SYS_IMP_CPUACTLR_EL3);
+FUNC_SYSREG_SMC_RW(IMP_CPUPPMCR_EL3, SYS_IMP_CPUPPMCR_EL3);
+FUNC_SYSREG_SMC_RW(IMP_CPUPPMCR2_EL3, SYS_IMP_CPUPPMCR2_EL3);
+FUNC_SYSREG_SMC_RW(IMP_CPUPPMCR4_EL3, SYS_IMP_CPUPPMCR4_EL3);
+FUNC_SYSREG_SMC_RW(IMP_CPUPPMCR5_EL3, SYS_IMP_CPUPPMCR5_EL3);
+FUNC_SYSREG_SMC_RW(IMP_CPUPPMCR6_EL3, SYS_IMP_CPUPPMCR6_EL3);
+
+// System registers
+static struct reg_desc system_regs[] = {
+	/* CONTROL */
+	REG_DESC_SYSREG_S_RW(VNCR_EL2),
+
+	/* ID */
+	REG_DESC_SYSREG_S_RO(CCSIDR_EL1),
+	REG_DESC_SYSREG_S_RO(CLIDR_EL1),
+	REG_DESC_SYSREG_S_RO(CSSELR_EL1),
+	REG_DESC_SYSREG_S_RO(CTR_EL0),
+	REG_DESC_SYSREG_S_RO(DCZID_EL0),
+	REG_DESC_SYSREG_S_RO(ID_AA64AFR0_EL1),
+	REG_DESC_SYSREG_S_RO(ID_AA64AFR1_EL1),
+	REG_DESC_SYSREG_S_RO(ID_AA64DFR0_EL1),
+	REG_DESC_SYSREG_S_RO(ID_AA64DFR1_EL1),
+	REG_DESC_SYSREG_S_RO(ID_AA64ISAR0_EL1),
+	REG_DESC_SYSREG_S_RO(ID_AA64ISAR1_EL1),
+	REG_DESC_SYSREG_S_RO(ID_AA64MMFR0_EL1),
+	REG_DESC_SYSREG_S_RO(ID_AA64MMFR1_EL1),
+	REG_DESC_SYSREG_S_RO(ID_AA64PFR0_EL1),
+	REG_DESC_SYSREG_S_RO(ID_AA64PFR1_EL1),
+
+	/* reset */
+	REG_DESC_SYSREG_SMC_RW(RMR_EL3),
+
+	/* implementation defined */
+	REG_DESC_SYSREG_S_RW(IMP_CPUECTLR_EL1),
+	REG_DESC_SYSREG_SMC_RW(IMP_CPUACTLR_EL3),
+	REG_DESC_SYSREG_SMC_RW(IMP_CPUPPMCR_EL3),
+	REG_DESC_SYSREG_SMC_RW(IMP_CPUPPMCR2_EL3),
+	REG_DESC_SYSREG_SMC_RW(IMP_CPUPPMCR4_EL3),
+	REG_DESC_SYSREG_SMC_RW(IMP_CPUPPMCR5_EL3),
+	REG_DESC_SYSREG_SMC_RW(IMP_CPUPPMCR6_EL3),
+};
+
+static struct attribute *id_attrs[] = {
+	REG_CTRL_ATTR_RO(CCSIDR_EL1, reg_show),
+	REG_CTRL_ATTR_RO(CLIDR_EL1, reg_show),
+	REG_CTRL_ATTR_RO(CSSELR_EL1, reg_show),
+	REG_CTRL_ATTR_RO(CTR_EL0, reg_show),
+	REG_CTRL_ATTR_RO(DCZID_EL0, reg_show),
+	REG_CTRL_ATTR_RO(ID_AA64AFR0_EL1, reg_show),
+	REG_CTRL_ATTR_RO(ID_AA64AFR1_EL1, reg_show),
+	REG_CTRL_ATTR_RO(ID_AA64DFR0_EL1, reg_show),
+	REG_CTRL_ATTR_RO(ID_AA64DFR1_EL1, reg_show),
+	REG_CTRL_ATTR_RO(ID_AA64ISAR0_EL1, reg_show),
+	REG_CTRL_ATTR_RO(ID_AA64ISAR1_EL1, reg_show),
+	REG_CTRL_ATTR_RO(ID_AA64MMFR0_EL1, reg_show),
+	REG_CTRL_ATTR_RO(ID_AA64MMFR1_EL1, reg_show),
+	REG_CTRL_ATTR_RO(ID_AA64PFR0_EL1, reg_show),
+	REG_CTRL_ATTR_RO(ID_AA64PFR1_EL1, reg_show),
+	NULL,
+};
+
+static struct attribute_group id_attr_group = {
+	.attrs = id_attrs,
+	.name = "id"
+};
+
+static struct attribute *control_attrs[] = {
+	REG_CTRL_ATTR_RW(VNCR_EL2, reg_show, reg_store),
+	NULL,
+};
+
+static struct attribute_group control_attr_group = {
+	.attrs = control_attrs,
+	.name = "control"
+};
+
+static struct attribute *reset_attrs[] = {
+	REG_CTRL_ATTR_RW(RMR_EL3, reg_show, reg_store),
+	NULL,
+};
+
+static struct attribute_group reset_attr_group = {
+	.attrs = reset_attrs,
+	.name = "reset"
+};
+
+static struct attribute *implementation_defined_attrs[] = {
+	REG_CTRL_ATTR_RW(IMP_CPUECTLR_EL1, reg_show, reg_store),
+	REG_CTRL_ATTR_RW(IMP_CPUACTLR_EL3, reg_show, reg_store),
+	REG_CTRL_ATTR_RW(IMP_CPUPPMCR_EL3, reg_show, reg_store),
+	REG_CTRL_ATTR_RW(IMP_CPUPPMCR2_EL3, reg_show, reg_store),
+	REG_CTRL_ATTR_RW(IMP_CPUPPMCR4_EL3, reg_show, reg_store),
+	REG_CTRL_ATTR_RW(IMP_CPUPPMCR5_EL3, reg_show, reg_store),
+	REG_CTRL_ATTR_RW(IMP_CPUPPMCR6_EL3, reg_show, reg_store),
+	NULL,
+};
+
+static struct attribute_group implementation_defined_attr_group = {
+	.attrs = implementation_defined_attrs,
+	.name = "implementation_defined"
+};
+
+static const struct attribute_group *system_attr_groups[] = {
+	&id_attr_group,
+	&control_attr_group,
+	&reset_attr_group,
+	&implementation_defined_attr_group,
+	NULL,
+};
+
+static struct reg_desc *get_reg_desc(const char *group, const char *name)
+{
+	struct reg_desc *regs = NULL;
+	int size = 0, i;
+
+	if (strcmp(group, "system") == 0) {
+		regs = system_regs;
+		size = ARRAY_SIZE(system_regs);
+	}
+
+	if (regs) {
+		for (i = 0; i < size; i++) {
+			if (strcmp(name, regs[i].name) == 0)
+				return &regs[i];
+		}
+	}
+
+	return NULL;
+}
+
+static ssize_t reg_show(struct kobject *kobj, struct kobj_attribute *attr,
+	char *buf)
+{
+	struct reg_desc *reg = NULL;
+
+	reg = get_reg_desc(kobject_name(kobj), attr->attr.name);
+
+	if (reg && reg->read != NULL)
+		return sprintf(buf, "0x%llx\n", reg->read());
+
+	return -EINVAL;
+}
+
+static ssize_t reg_store(struct kobject *kobj, struct kobj_attribute *attr,
+	const char *buf, size_t count)
+{
+	struct reg_desc *reg = NULL;
+	u64 val;
+
+	reg = get_reg_desc(kobject_name(kobj), attr->attr.name);
+
+	if (reg && reg->write != NULL && !kstrtoull(buf, 0, &val)) {
+		reg->write(val);
+		return count;
+	}
+	return -EINVAL;
+}
+
+static int __init reg_init(void)
+{
+	int retval = -1;
+
+	reg_kobj = kobject_create_and_add("reg_ctrl", kernel_kobj);
+	if (!reg_kobj)
+		return -ENOMEM;
+
+	system_kobj = kobject_create_and_add("system", reg_kobj);
+	if (!system_kobj)
+		goto fail_system;
+
+	retval = sysfs_create_groups(system_kobj, system_attr_groups);
+	if (retval)
+		goto fail_system_attr_groups;
+
+	return 0;
+
+fail_system_attr_groups:
+	if (system_kobj)
+		kobject_put(system_kobj);
+fail_system:
+	if (reg_kobj)
+		kobject_put(reg_kobj);
+	return retval;
+}
+
+static void __exit reg_exit(void)
+{
+	if (system_kobj) {
+		sysfs_remove_groups(system_kobj, system_attr_groups);
+		kobject_put(system_kobj);
+	}
+
+	if (reg_kobj)
+		kobject_put(reg_kobj);
+}
+
+module_init(reg_init);
+module_exit(reg_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("jaguarmicro");
+MODULE_DESCRIPTION("reg_ctrl is a tool to read/write ARM64 system registers");
diff --git a/tools/arch/arm64/tools/reg_ctrl/reg_ctrl.h b/tools/arch/arm64/tools/reg_ctrl/reg_ctrl.h
new file mode 100644
index 000000000000..f6f96cf5d4af
--- /dev/null
+++ b/tools/arch/arm64/tools/reg_ctrl/reg_ctrl.h
@@ -0,0 +1,98 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef __REG_CTRL_H
+#define __REG_CTRL_H
+#include <linux/arm-smccc.h>
+
+#define MAX_REG_NAME_LEN	32
+
+struct reg_desc {
+	char name[MAX_REG_NAME_LEN];
+	u64 (*read)(void);
+	void (*write)(u64 val);
+};
+
+#define _REG_DESC_SYSREG(nm, rd, wr)					\
+	{								\
+		.name = __stringify(nm),				\
+		.read = rd,						\
+		.write = wr,						\
+	}
+
+#define REG_DESC_SYSREG_RW(nm)						\
+	_REG_DESC_SYSREG(nm, sysreg_read_##nm, sysreg_write_##nm)
+
+#define REG_DESC_SYSREG_RO(nm)						\
+	_REG_DESC_SYSREG(nm, sysreg_read_##nm, NULL)
+
+#define REG_DESC_SYSREG_S_RW(nm)					\
+	_REG_DESC_SYSREG(nm, sysreg_read_s_##nm, sysreg_write_s_##nm)
+
+#define REG_DESC_SYSREG_S_RO(nm)					\
+	_REG_DESC_SYSREG(nm, sysreg_read_s_##nm, NULL)
+
+#define REG_DESC_SYSREG_SMC_RW(nm)					\
+	_REG_DESC_SYSREG(nm, sysreg_smc_read_##nm, sysreg_smc_write_##nm)
+
+#define REG_DESC_SYSREG_SMC_RO(nm)					\
+	_REG_DESC_SYSREG(nm, sysreg_smc_read_##nm, NULL)
+
+
+#define FUNC_SYSREG_RW(nm)						\
+static u64 sysreg_read_##nm(void)					\
+{									\
+	return read_sysreg(nm);						\
+}									\
+static void sysreg_write_##nm(u64 val)					\
+{									\
+	write_sysreg(val, nm);						\
+}
+
+#define FUNC_SYSREG_RO(nm)						\
+static u64 sysreg_read_##nm(void)					\
+{									\
+	return read_sysreg(nm);						\
+}
+
+#define FUNC_SYSREG_S_RW(nm, sys)					\
+static u64 sysreg_read_s_##nm(void)					\
+{									\
+	return read_sysreg_s(sys);					\
+}									\
+static void sysreg_write_s_##nm(u64 val)				\
+{									\
+	write_sysreg_s(val, sys);					\
+}
+
+#define FUNC_SYSREG_S_RO(nm, sys)					\
+static u64 sysreg_read_s_##nm(void)					\
+{									\
+	return read_sysreg_s(sys);					\
+}
+
+#define FUNC_SYSREG_SMC_RW(nm, reg)					\
+static u64 sysreg_smc_read_##nm(void)					\
+{									\
+	struct arm_smccc_res res;					\
+	arm_smccc_smc(SMCCC_OEM_REG_CTRL_READ_REG, reg,			\
+			0, 0, 0, 0, 0, 0, &res);			\
+	return res.a0;							\
+}									\
+static void sysreg_smc_write_##nm(u64 val)				\
+{									\
+	struct arm_smccc_res res;					\
+	arm_smccc_smc(SMCCC_OEM_REG_CTRL_WRITE_REG, val, reg,		\
+			0, 0, 0, 0, 0, &res);				\
+}
+
+
+#define _REG_CTRL_ATTR(name, mode, show, store)				\
+	(&((struct kobj_attribute) __ATTR(name, mode, show, store)).attr)
+
+#define REG_CTRL_ATTR_RW(name, show, store)				\
+	_REG_CTRL_ATTR(name, 0664, show, store)
+
+#define REG_CTRL_ATTR_RO(name, show)					\
+	_REG_CTRL_ATTR(name, 0444, show, NULL)
+
+
+#endif /* __REG_CTRL_H */
diff --git a/tools/arch/arm64/tools/reg_ctrl/sysreg.h b/tools/arch/arm64/tools/reg_ctrl/sysreg.h
new file mode 100644
index 000000000000..4d93eedb6b7a
--- /dev/null
+++ b/tools/arch/arm64/tools/reg_ctrl/sysreg.h
@@ -0,0 +1,20 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+#ifndef __LOCAL_SYSREG_H_
+#define __LOCAL_SYSREG_H_
+#include <asm/sysreg.h>
+
+#ifndef SYS_RMR_EL3
+#define SYS_RMR_EL3			sys_reg(3, 6, 12, 0, 2)
+#endif
+
+#define SYS_IMP_CPUECTLR_EL1		sys_reg(3, 0, 15, 1, 4)
+#define SYS_IMP_CPUACTLR_EL3		sys_reg(3, 6, 15, 4, 0)
+#define SYS_IMP_CPUPPMCR_EL3		sys_reg(3, 6, 15, 2, 0)
+#define SYS_IMP_CPUPPMCR2_EL3		sys_reg(3, 6, 15, 2, 1)
+#define SYS_IMP_CPUPPMCR4_EL3		sys_reg(3, 6, 15, 2, 4)
+#define SYS_IMP_CPUPPMCR5_EL3		sys_reg(3, 6, 15, 2, 5)
+#define SYS_IMP_CPUPPMCR6_EL3		sys_reg(3, 6, 15, 2, 6)
+
+#define SYS_VNCR_EL2			sys_reg(3, 4, 2, 2, 0)
+
+#endif
diff --git a/tools/arch/arm64/tools/reg_ctrl/testme.sh b/tools/arch/arm64/tools/reg_ctrl/testme.sh
new file mode 100644
index 000000000000..92a6f79ee866
--- /dev/null
+++ b/tools/arch/arm64/tools/reg_ctrl/testme.sh
@@ -0,0 +1,17 @@
+#!/bin/bash
+# SPDX-License-Identifier: GPL-2.0
+rmmod reg_ctrl.ko
+insmod reg_ctrl.ko
+
+echo "start test read, expect values:"
+echo "
+0x0
+0x4000240340543001
+0x0
+0x2000315a10000045
+0x7000336f
+0x15401480136
+0x70300
+"
+cat /sys/kernel/reg_ctrl/system/implementation_defined/IMP_CPU*
+
diff --git a/tools/arch/arm64/tools/reg_ctrl/testme_all_cpu.sh b/tools/arch/arm64/tools/reg_ctrl/testme_all_cpu.sh
new file mode 100644
index 000000000000..7000508adeeb
--- /dev/null
+++ b/tools/arch/arm64/tools/reg_ctrl/testme_all_cpu.sh
@@ -0,0 +1,15 @@
+#!/bin/bash
+# SPDX-License-Identifier: GPL-2.0
+
+rmmod reg_ctrl.ko
+insmod reg_ctrl.ko
+NR_CORES=16
+
+i=0
+while [ $i -lt $NR_CORES ]
+do
+	echo "run on core: $i"
+	taskset -c $i cat /sys/kernel/reg_ctrl/system/control/VNCR_EL2
+	let "i=i+1"
+	echo ""
+done
-- 
2.17.1

Powered by blists - more mailing lists