Initial support for virtual cpufreq devicemain

Work-in-progress, beware! It might eat your pet hamster.

Signed-off-by: Will Deacon <will@kernel.org>

4 files changed, 133 insertions, 1 deletions

diff --git a/Makefile b/Makefile
index 5d9bf59..79bc524 100644
--- a/Makefile
+++ b/Makefile
@@ -7,7 +7,7 @@ export INC=-I /usr/include/$(shell uname -m)-linux-gnu -I $(CURDIR)/include
 export CFLAGS=-target bpf -Wall -c -O2 -g $(INC)
 export ASFLAGS=$(CFLAGS)
 
-SUBDIRS=pl031
+SUBDIRS=pl031 vcpufreq
 TARGETS=all clean
 
 $(TARGETS): $(SUBDIRS)
diff --git a/vcpufreq/Makefile b/vcpufreq/Makefile
new file mode 100644
index 0000000..df17185
--- /dev/null
+++ b/vcpufreq/Makefile
@@ -0,0 +1,26 @@
+# SPDX-License-Identifier: GPL-2.0-only
+
+TARGETS=vcpufreq-device.o vcpufreq.skel.h
+
+all: $(TARGETS)
+
+# I can't find a linker that will link these for me (either SEGV, internal
+# linker error or some error in free()). So let's do it by hand...
+vcpufreq-device.o: vcpufreq.o vcpufreq-note.o
+	$(OBJCOPY) --dump-section=.note.kvm-bpf.mmio-device=vcpufreq-note-section.o vcpufreq-note.o
+	$(OBJCOPY) --add-section .note.kvm-bpf.mmio-device=vcpufreq-note-section.o vcpufreq.o vcpufreq.tmp.o
+	$(OBJCOPY) --set-section-alignment=.note.kvm-bpf.mmio-device=4		\
+		   --set-section-flags=.note.kvm-bpf.mmio-device=alloc,readonly	\
+		   vcpufreq.tmp.o $@
+
+vcpufreq.skel.h: vcpufreq.o
+ifeq (,$(BPFTOOL))
+	$(error $(BPFTOOL) not found; cannot generate skeleton file)
+else
+	$(BPFTOOL) gen skeleton $< > $@
+endif
+
+clean:
+	rm -f *.o $(TARGETS)
+
+.PHONY: all clean
diff --git a/vcpufreq/vcpufreq-note.S b/vcpufreq/vcpufreq-note.S
new file mode 100644
index 0000000..9df92ab
--- /dev/null
+++ b/vcpufreq/vcpufreq-note.S
@@ -0,0 +1,29 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+
+#include "kvm-bpf-elf.h"
+
+.pushsection NT_KVM_BPF_SECTION_NAME, "a"
+	.p2align	2
+	.long		2f - 1f
+	.long		4f - 3f
+	.long		NT_KVM_BPF_DEVICE_PROP_TYPE_MMIO
+1:	.string		NT_KVM_BPF_DEVICE_NAME
+2:	.p2align	2
+3:
+	.long		NT_KVM_BPF_DEVICE_MMIO_DT_COMPATIBLE
+	.string		"virtual,kvm-cpufreq"
+4:	.p2align	2
+.popsection
+
+.pushsection NT_KVM_BPF_SECTION_NAME, "a"
+	.p2align	2
+	.long		2f - 1f
+	.long		4f - 3f
+	.long		NT_KVM_BPF_DEVICE_PROP_TYPE_MMIO
+1:	.string		NT_KVM_BPF_DEVICE_NAME
+2:	.p2align	2
+3:
+	.long		NT_KVM_BPF_DEVICE_MMIO_SIZE
+	.long		0x1000 /* XXX: really just 8 bytes per vCPU */
+4:	.p2align	2
+.popsection
diff --git a/vcpufreq/vcpufreq.c b/vcpufreq/vcpufreq.c
new file mode 100644
index 0000000..0188405
--- /dev/null
+++ b/vcpufreq/vcpufreq.c
@@ -0,0 +1,77 @@
+// SPDX-License-Identifier: GPL-2.0-only
+
+#include <errno.h>
+#include <linux/bpf.h>
+#include <linux/types.h>
+#include <bpf/bpf_helpers.h>
+
+#include "kvm-bpf-elf.h"
+
+#define __stringify_1(x...)	#x
+#define __stringify(x...)	__stringify_1(x)
+
+#define OFFSET_TO_REG(off)	((off) & 0x7)
+#define OFFSET_TO_VCPU(off)	((off) >> 3)
+
+#define REG_CUR_FREQ		0x0
+#define REG_SET_FREQ		0x4
+
+struct bpf_kvm_io_ctx {
+	__u8	buf[8];
+	__u64	offset;
+	__u8	len;
+	__u32	:24;
+	__u32	vcpu_id;
+};
+
+static __u32 (*bpf_get_cpu_freq)(__u32 cpu) = (void *) 212;
+static __u32 (*bpf_get_cpu_max_hw_freq)(__u32 cpu) = (void *) 213;
+static __u64 (*bpf_get_cpu_scale)(__u32 cpu) = (void *) 214;
+static int (*bpf_set_current_uclamp)(__u32 sched_util_min, __u32 sched_util_max) = (void *) 215;
+
+SEC("struct_ops/" __stringify(KVM_BPF_READ_PROG_NAME))
+int KVM_BPF_READ_PROG_NAME(struct bpf_kvm_io_ctx *ctx)
+{
+	__u32 vcpu = OFFSET_TO_VCPU(ctx->offset);
+	__u32 reg = OFFSET_TO_REG(ctx->offset);
+	__u32 cpu = bpf_get_smp_processor_id();
+
+	if (vcpu != ctx->vcpu_id)
+		return -EPERM;
+
+	switch (reg) {
+	case REG_CUR_FREQ:
+		*((__u32 *)ctx->buf) = bpf_get_cpu_freq(cpu);
+		break;
+	default:
+		return -ENXIO;
+	}
+
+	return 0;
+}
+
+SEC("struct_ops/" __stringify(KVM_BPF_WRITE_PROG_NAME))
+int KVM_BPF_WRITE_PROG_NAME(struct bpf_kvm_io_ctx *ctx)
+{
+	__u32 vcpu = OFFSET_TO_VCPU(ctx->offset);
+	__u32 reg = OFFSET_TO_REG(ctx->offset);
+	__u32 cpu = bpf_get_smp_processor_id();
+	__u32 sched_util_min;
+	__u32 val;
+
+	if (vcpu != ctx->vcpu_id)
+		return -EPERM;
+
+	switch (reg) {
+	case REG_SET_FREQ:
+		val = *((__u32 *)ctx->buf);
+		val *= bpf_get_cpu_scale(cpu);
+		sched_util_min = val / bpf_get_cpu_max_hw_freq(cpu);
+
+		return bpf_set_current_uclamp(sched_util_min, -1);
+	default:
+		return -ENXIO;
+	}
+
+	return 0;
+}