Add cpuid FreeBSD kernel module for ARM CPUs

drivers/arm/CMakeLists.txt

@@ -3,6 +3,8 @@ if(LIBCPUID_DRIVER_DEBUG)
   add_definitions(-DLIBCPUID_DRIVER_DEBUG)
 endif(LIBCPUID_DRIVER_DEBUG)
 
-if("${CMAKE_SYSTEM_NAME}" STREQUAL "Linux")
+if("${CMAKE_SYSTEM_NAME}" STREQUAL "FreeBSD")
+  add_subdirectory(freebsd)
+elseif("${CMAKE_SYSTEM_NAME}" STREQUAL "Linux")
   add_subdirectory(linux)
 endif()

drivers/arm/freebsd/CMakeLists.txt

@@ -0,0 +1,17 @@
+set(DRIVER_NAME "cpuid")
+set(DRIVER_SOURCE "${DRIVER_NAME}.c")
+set(DRIVER_OBJECT "${DRIVER_NAME}.o")
+set(DRIVER_MODULE "${DRIVER_NAME}.ko")
+
+configure_file("${DRIVER_SOURCE}" "${CMAKE_CURRENT_BINARY_DIR}/${DRIVER_SOURCE}" COPYONLY)
+configure_file(Makefile.in "${CMAKE_CURRENT_BINARY_DIR}/Makefile_kmod")
+
+add_custom_command(OUTPUT "${DRIVER_MODULE}"
+  COMMAND "${CMAKE_MAKE_PROGRAM}" -f Makefile_kmod
+  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  DEPENDS "${DRIVER_SOURCE}"
+  COMMENT "Building CPUID kernel module for ARM..."
+  VERBATIM
+)
+
+add_custom_target(driver-arm-cpuid ALL DEPENDS "${DRIVER_MODULE}")

drivers/arm/freebsd/Makefile.in

@@ -0,0 +1,9 @@
+KMOD=@DRIVER_NAME@
+
+SRCS= \
+	@DRIVER_SOURCE@
+
+CFLAGS+= \
+	-I@CMAKE_SOURCE_DIR@/libcpuid
+
+.include <bsd.kmod.mk>

drivers/arm/freebsd/cpuid.c

@@ -0,0 +1,247 @@
+/*
+ * Copyright 2024  Veselin Georgiev,
+ * anrieffNOSPAM @ mgail_DOT.com (convert to gmail)
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/* This kernel module is inspired on cpuctl: https://github.com/freebsd/freebsd-src/blob/master/sys/dev/cpuctl/cpuctl.c */
+
+#include <sys/param.h>
+#include <sys/systm.h>
+#include <sys/conf.h>
+#include <sys/ioccom.h>
+#include <sys/malloc.h>
+#include <sys/module.h>
+#include <sys/mutex.h>
+#include <sys/proc.h>
+#include <sys/queue.h>
+#include <sys/sched.h>
+#include <sys/smp.h>
+#include <sys/pmckern.h>
+
+#include "libcpuid_arm_driver.h"
+
+#define	CPUID_VERSION 1
+
+#ifdef LIBCPUID_DRIVER_DEBUG
+# define	DPRINTF(format,...) printf(format, __VA_ARGS__);
+#else
+# define	DPRINTF(...)
+#endif
+
+#if defined(__arm__)
+# define cpuid_read_sysreg(aarch32, aarch64, value) asm volatile("mrc " aarch32 : "=r" (value))
+#elif defined(__aarch64__)
+# define cpuid_read_sysreg(aarch32, aarch64, value) { uint64_t __val; asm volatile("mrs %0, " aarch64 : "=r" (__val)); value = __val; }
+#else
+# error This platform is not supported by this kernel module
+#endif
+
+static d_ioctl_t cpuid_ioctl;
+static struct cdev **cpuid_devs;
+static MALLOC_DEFINE(M_CPUID, "cpuid", "CPUID buffer");
+
+/*
+ * This function checks if specified cpu enabled or not.
+ */
+static int
+cpu_enabled(int cpu)
+{
+	return (pmc_cpu_is_disabled(cpu) == 0);
+}
+
+/*
+ * Check if the current thread is bound to a specific cpu.
+ */
+static int
+cpu_sched_is_bound(struct thread *td)
+{
+	int ret;
+
+	thread_lock(td);
+	ret = sched_is_bound(td);
+	thread_unlock(td);
+	return (ret);
+}
+
+/*
+ * Switch to target cpu to run.
+ */
+static void
+set_cpu(int cpu, struct thread *td)
+{
+	KASSERT(cpu >= 0 && cpu <= mp_maxid && cpu_enabled(cpu), ("[cpuid,%d]: bad cpu number %d", __LINE__, cpu));
+	thread_lock(td);
+	sched_bind(td, cpu);
+	thread_unlock(td);
+	KASSERT(td->td_oncpu == cpu, ("[cpuid,%d]: cannot bind to target cpu %d on cpu %d", __LINE__, cpu, td->td_oncpu));
+}
+
+static void
+restore_cpu(int oldcpu, int is_bound, struct thread *td)
+{
+	KASSERT(oldcpu >= 0 && oldcpu <= mp_maxid && cpu_enabled(oldcpu), ("[cpuid,%d]: bad cpu number %d", __LINE__, oldcpu));
+	thread_lock(td);
+	if (is_bound == 0)
+		sched_unbind(td);
+	else
+		sched_bind(td, oldcpu);
+	thread_unlock(td);
+}
+
+/*
+ * Actually perform register read.
+ */
+static int
+__read_reg_on_cpu(int cpu, struct read_reg_t *read_reg, u_long cmd, struct thread *td)
+{
+	int is_bound = 0;
+	int oldcpu;
+
+	KASSERT(cpu >= 0 && cpu <= mp_maxid, ("[cpuid,%d]: bad cpu number %d", __LINE__, cpu));
+	oldcpu = td->td_oncpu;
+	is_bound = cpu_sched_is_bound(td);
+	set_cpu(cpu, td);
+
+	DPRINTF("[cpuid,%d]: operating on request %d\n", __LINE__, read_reg->request);
+	read_reg->value_64b = 0;
+	switch(read_reg->request)
+	{
+		case REQ_MIDR:     cpuid_read_sysreg("p15, 0, %0, c0, c0, 0", AARCH64_REG_MIDR_EL1,   read_reg->value_64b); break;
+		case REQ_MPIDR:    cpuid_read_sysreg("p15, 0, %0, c0, c0, 5", AARCH64_REG_MPIDR_EL1,  read_reg->value_64b); break;
+		case REQ_REVIDR:   cpuid_read_sysreg("p15, 0, %0, c0, c0, 6", AARCH64_REG_REVIDR_EL1, read_reg->value_64b); break;
+		case REQ_ID_AFR0:  cpuid_read_sysreg("p15, 0, %0, c0, c1, 3", AARCH64_REG_ID_AFR0,    read_reg->value_32b); break;
+		case REQ_ID_DFR0:  cpuid_read_sysreg("p15, 0, %0, c0, c1, 2", AARCH64_REG_ID_DFR0,    read_reg->value_32b); break;
+		case REQ_ID_DFR1:  cpuid_read_sysreg("p15, 0, %0, c0, c3, 5", AARCH64_REG_ID_DFR1,    read_reg->value_32b); break;
+		case REQ_ID_ISAR0: cpuid_read_sysreg("p15, 0, %0, c0, c2, 0", AARCH64_REG_ID_ISAR0,   read_reg->value_32b); break;
+		case REQ_ID_ISAR1: cpuid_read_sysreg("p15, 0, %0, c0, c2, 1", AARCH64_REG_ID_ISAR1,   read_reg->value_32b); break;
+		case REQ_ID_ISAR2: cpuid_read_sysreg("p15, 0, %0, c0, c2, 2", AARCH64_REG_ID_ISAR2,   read_reg->value_32b); break;
+		case REQ_ID_ISAR3: cpuid_read_sysreg("p15, 0, %0, c0, c2, 3", AARCH64_REG_ID_ISAR3,   read_reg->value_32b); break;
+		case REQ_ID_ISAR4: cpuid_read_sysreg("p15, 0, %0, c0, c2, 4", AARCH64_REG_ID_ISAR4,   read_reg->value_32b); break;
+		case REQ_ID_ISAR5: cpuid_read_sysreg("p15, 0, %0, c0, c2, 5", AARCH64_REG_ID_ISAR5,   read_reg->value_32b); break;
+		case REQ_ID_ISAR6: cpuid_read_sysreg("p15, 0, %0, c0, c2, 7", AARCH64_REG_ID_ISAR6,   read_reg->value_32b); break;
+		case REQ_ID_MMFR0: cpuid_read_sysreg("p15, 0, %0, c0, c1, 4", AARCH64_REG_ID_MMFR0,   read_reg->value_32b); break;
+		case REQ_ID_MMFR1: cpuid_read_sysreg("p15, 0, %0, c0, c1, 5", AARCH64_REG_ID_MMFR1,   read_reg->value_32b); break;
+		case REQ_ID_MMFR2: cpuid_read_sysreg("p15, 0, %0, c0, c1, 6", AARCH64_REG_ID_MMFR2,   read_reg->value_32b); break;
+		case REQ_ID_MMFR3: cpuid_read_sysreg("p15, 0, %0, c0, c1, 7", AARCH64_REG_ID_MMFR3,   read_reg->value_32b); break;
+		case REQ_ID_MMFR4: cpuid_read_sysreg("p15, 0, %0, c0, c2, 6", AARCH64_REG_ID_MMFR4,   read_reg->value_32b); break;
+		case REQ_ID_MMFR5: cpuid_read_sysreg("p15, 0, %0, c0, c3, 6", AARCH64_REG_ID_MMFR5,   read_reg->value_32b); break;
+		case REQ_ID_PFR0:  cpuid_read_sysreg("p15, 0, %0, c0, c1, 0", AARCH64_REG_ID_PFR0,    read_reg->value_32b); break;
+		case REQ_ID_PFR1:  cpuid_read_sysreg("p15, 0, %0, c0, c1, 1", AARCH64_REG_ID_PFR1,    read_reg->value_32b); break;
+		case REQ_ID_PFR2:  cpuid_read_sysreg("p15, 0, %0, c0, c3, 4", AARCH64_REG_ID_PFR2,    read_reg->value_32b); break;
+#if defined(__aarch64__)
+		case REQ_ID_AA64AFR0:  cpuid_read_sysreg(NULL, AARCH64_REG_ID_AA64AFR0_EL1,  read_reg->value_64b); break;
+		case REQ_ID_AA64AFR1:  cpuid_read_sysreg(NULL, AARCH64_REG_ID_AA64AFR1_EL1,  read_reg->value_64b); break;
+		case REQ_ID_AA64DFR0:  cpuid_read_sysreg(NULL, AARCH64_REG_ID_AA64DFR0_EL1,  read_reg->value_64b); break;
+		case REQ_ID_AA64DFR1:  cpuid_read_sysreg(NULL, AARCH64_REG_ID_AA64DFR1_EL1,  read_reg->value_64b); break;
+		case REQ_ID_AA64ISAR0: cpuid_read_sysreg(NULL, AARCH64_REG_ID_AA64ISAR0_EL1, read_reg->value_64b); break;
+		case REQ_ID_AA64ISAR1: cpuid_read_sysreg(NULL, AARCH64_REG_ID_AA64ISAR1_EL1, read_reg->value_64b); break;
+		case REQ_ID_AA64ISAR2: cpuid_read_sysreg(NULL, AARCH64_REG_ID_AA64ISAR2_EL1, read_reg->value_64b); break;
+		case REQ_ID_AA64MMFR0: cpuid_read_sysreg(NULL, AARCH64_REG_ID_AA64MMFR0_EL1, read_reg->value_64b); break;
+		case REQ_ID_AA64MMFR1: cpuid_read_sysreg(NULL, AARCH64_REG_ID_AA64MMFR1_EL1, read_reg->value_64b); break;
+		case REQ_ID_AA64MMFR2: cpuid_read_sysreg(NULL, AARCH64_REG_ID_AA64MMFR2_EL1, read_reg->value_64b); break;
+		case REQ_ID_AA64MMFR3: cpuid_read_sysreg(NULL, AARCH64_REG_ID_AA64MMFR3_EL1, read_reg->value_64b); break;
+		case REQ_ID_AA64MMFR4: cpuid_read_sysreg(NULL, AARCH64_REG_ID_AA64MMFR4_EL1, read_reg->value_64b); break;
+		case REQ_ID_AA64PFR0:  cpuid_read_sysreg(NULL, AARCH64_REG_ID_AA64PFR0_EL1,  read_reg->value_64b); break;
+		case REQ_ID_AA64PFR1:  cpuid_read_sysreg(NULL, AARCH64_REG_ID_AA64PFR1_EL1,  read_reg->value_64b); break;
+		case REQ_ID_AA64PFR2:  cpuid_read_sysreg(NULL, AARCH64_REG_ID_AA64PFR2_EL1,  read_reg->value_64b); break;
+		case REQ_ID_AA64SMFR0: cpuid_read_sysreg(NULL, AARCH64_REG_ID_AA64SMFR0_EL1, read_reg->value_64b); break;
+		case REQ_ID_AA64ZFR0:  cpuid_read_sysreg(NULL, AARCH64_REG_ID_AA64ZFR0_EL1,  read_reg->value_64b); break;
+#endif /* __aarch64__ */
+		default:
+			read_reg->request = REQ_INVALID;
+			printf("[cpuid,%d]: unknown operation requested: %d", __LINE__, read_reg->request);
+			break;
+	}
+	DPRINTF("[cpuid,%d]: set value 0x%016lX for cpu number %d\n", __LINE__, read_reg->value_64b, cpu);
+
+	restore_cpu(oldcpu, is_bound, td);
+	return 0;
+}
+
+int
+cpuid_ioctl(struct cdev *dev, u_long cmd, caddr_t data, int flags, struct thread *td)
+{
+	int cpu, ret;
+
+	cpu = dev2unit(dev);
+	if (cpu > mp_maxid || !cpu_enabled(cpu)) {
+		DPRINTF("[cpuid,%d]: bad cpu number %d\n", __LINE__, cpu);
+		return (ENXIO);
+	}
+
+	DPRINTF("[cpuid,%d]: received ioctl %lu for cpu number %d\n", __LINE__, cmd, cpu);
+	switch (cmd) {
+		case ARM_IOC_READ_REG:
+			ret = __read_reg_on_cpu(cpu, (struct read_reg_t *)data, cmd, td);
+			break;
+
+		default:
+			ret = EINVAL;
+			break;
+	}
+
+	return (ret);
+}
+
+static struct cdevsw cpuid_cdevsw = {
+	.d_name =       "cpuid",
+	.d_version =    D_VERSION,
+	.d_ioctl =      cpuid_ioctl,
+};
+
+static int
+cpuid_modevent(module_t mod __unused, int type, void *data __unused)
+{
+	int cpu;
+
+	switch(type) {
+		case MOD_LOAD:
+			DPRINTF("[cpuid,%d]: load module\n", __LINE__);
+			cpuid_devs = malloc(sizeof(*cpuid_devs) * (mp_maxid + 1), M_CPUID, M_WAITOK | M_ZERO);
+			CPU_FOREACH(cpu)
+				if (cpu_enabled(cpu))
+					cpuid_devs[cpu] = make_dev(&cpuid_cdevsw, cpu, UID_ROOT, GID_KMEM, 0640, "cpuid%d", cpu);
+			break;
+
+		case MOD_UNLOAD:
+			DPRINTF("[cpuid,%d]: unload module\n", __LINE__);
+			CPU_FOREACH(cpu) {
+				if (cpuid_devs[cpu] != NULL)
+					destroy_dev(cpuid_devs[cpu]);
+			}
+			free(cpuid_devs, M_CPUID);
+			break;
+
+		case MOD_SHUTDOWN:
+			break;
+
+		default:
+			return (EOPNOTSUPP);
+	}
+
+	return (0);
+}
+
+DEV_MODULE(cpuid, cpuid_modevent, NULL);
+MODULE_VERSION(cpuid, CPUID_VERSION);