From 93a6e922950838f805a593c9dba613fededeee94 Mon Sep 17 00:00:00 2001
From: Kent McLeod <kent@kry10.com>
Date: Tue, 30 May 2023 16:42:55 +1000
Subject: [PATCH] elfloader: Add generic-timer driver

This driver just sets CNTVOFF_El2 to 0 if possible.
This can only be done if the elfloader is in hyp-mode, otherwise nothing
happens.

Signed-off-by: Kent McLeod <kent@kry10.com>
---
 elfloader-tool/CMakeLists.txt                 |  1 +
 elfloader-tool/include/drivers/common.h       |  1 +
 elfloader-tool/src/arch-arm/smp_boot.c        |  3 ++
 .../src/drivers/timer/generic_timer.c         | 34 +++++++++++++++++++
 4 files changed, 39 insertions(+)
 create mode 100644 elfloader-tool/src/drivers/timer/generic_timer.c

diff --git a/elfloader-tool/CMakeLists.txt b/elfloader-tool/CMakeLists.txt
index 2a4d7884..ac747c94 100644
--- a/elfloader-tool/CMakeLists.txt
+++ b/elfloader-tool/CMakeLists.txt
@@ -171,6 +171,7 @@ file(
         src/drivers/*.c
         src/drivers/smp/*.c
         src/drivers/uart/*.c
+        src/drivers/timer/*.c
         src/utils/*.c
         src/arch-${KernelArch}/*.c
         src/arch-${KernelArch}/*.S
diff --git a/elfloader-tool/include/drivers/common.h b/elfloader-tool/include/drivers/common.h
index 57d40d13..28e06441 100644
--- a/elfloader-tool/include/drivers/common.h
+++ b/elfloader-tool/include/drivers/common.h
@@ -26,6 +26,7 @@ enum driver_type {
     DRIVER_INVALID = 0,
     DRIVER_SMP,
     DRIVER_UART,
+    DRIVER_TIMER,
     DRIVER_MAX
 };
 
diff --git a/elfloader-tool/src/arch-arm/smp_boot.c b/elfloader-tool/src/arch-arm/smp_boot.c
index e25b5612..c39cf7f2 100644
--- a/elfloader-tool/src/arch-arm/smp_boot.c
+++ b/elfloader-tool/src/arch-arm/smp_boot.c
@@ -43,6 +43,9 @@ void non_boot_main(void)
     /* Initialise any platform-specific per-core state */
     non_boot_init();
 
+    /* Do any driver specific non_boot core init */
+    initialise_devices_non_boot();
+
 #ifndef CONFIG_ARM_HYPERVISOR_SUPPORT
     if (is_hyp_mode()) {
         extern void leave_hyp(void);
diff --git a/elfloader-tool/src/drivers/timer/generic_timer.c b/elfloader-tool/src/drivers/timer/generic_timer.c
new file mode 100644
index 00000000..77de6cc5
--- /dev/null
+++ b/elfloader-tool/src/drivers/timer/generic_timer.c
@@ -0,0 +1,34 @@
+/*
+ * Copyright 2020, Data61, CSIRO (ABN 41 687 119 230)
+ *
+ * SPDX-License-Identifier: GPL-2.0-only
+ */
+
+#include <devices_gen.h>
+#include <drivers/common.h>
+#include <mode/arm_generic_timer.h>
+
+#include <elfloader_common.h>
+
+
+static int generic_timer_init(UNUSED struct elfloader_device *dev, UNUSED void *match_data)
+{
+    reset_cntvoff();
+    return 0;
+}
+
+static const struct dtb_match_table generic_timer_matches[] = {
+    { .compatible = "arm,armv7-timer" },
+    { .compatible = "arm,armv8-timer" },
+    { .compatible = NULL /* sentinel */ },
+};
+
+static const struct elfloader_driver generic_timer = {
+    .match_table = generic_timer_matches,
+    .type = DRIVER_TIMER,
+    .init = &generic_timer_init,
+    .init_non_boot = &generic_timer_init,
+    .ops = NULL,
+};
+
+ELFLOADER_DRIVER(generic_timer);