From 1eb60a2a52bb14e6cc9f5bd0dc28f9964ef0ff19 Mon Sep 17 00:00:00 2001
From: TuxSH <1922548+TuxSH@users.noreply.github.com>
Date: Thu, 9 Jan 2020 19:24:05 +0000
Subject: [PATCH] thermosphere: add hypervisor timer code
---
thermosphere/Makefile | 4 +-
thermosphere/src/irq.c | 4 +
thermosphere/src/irq.h | 12 ++-
thermosphere/src/main.c | 3 +-
.../src/platform/qemu/interrupt_config.h | 9 +-
.../src/platform/tegra/interrupt_config.h | 17 ++--
thermosphere/src/sysreg.h | 6 +-
thermosphere/src/timer.c | 38 ++++++++
thermosphere/src/timer.h | 91 +++++++++++++++++++
thermosphere/src/utils.h | 2 +-
10 files changed, 169 insertions(+), 17 deletions(-)
create mode 100644 thermosphere/src/timer.c
create mode 100644 thermosphere/src/timer.h
diff --git a/thermosphere/Makefile b/thermosphere/Makefile
index 3fd78ae58..ce32c9784 100644
--- a/thermosphere/Makefile
+++ b/thermosphere/Makefile
@@ -144,8 +144,8 @@ ifeq ($(PLATFORM), qemu)
export QEMU := qemu-system-aarch64
-QEMUFLAGS := -nographic -machine virt,virtualization=on,gic-version=2 -cpu cortex-a57 -smp 4 -m 1024\
- -kernel thermosphere.elf -d unimp,guest_errors -semihosting-config enable,target=native -serial mon:stdio
+QEMUFLAGS := -nographic -machine virt,virtualization=on,accel=tcg,gic-version=2 -cpu cortex-a57 -smp 4 -m 1024\
+ -kernel thermosphere.elf -d unimp,guest_errors -semihosting-config enable,target=native -serial mon:stdio
qemu: all
@$(QEMU) $(QEMUFLAGS)
diff --git a/thermosphere/src/irq.c b/thermosphere/src/irq.c
index 472078ff5..b273595e5 100644
--- a/thermosphere/src/irq.c
+++ b/thermosphere/src/irq.c
@@ -18,6 +18,7 @@
#include "core_ctx.h"
#include "debug_log.h"
#include "vgic.h"
+#include "timer.h"
IrqManager g_irqManager = {0};
@@ -183,6 +184,9 @@ void handleIrqException(ExceptionStackFrame *frame, bool isLowerEl, bool isA32)
case GIC_IRQID_MAINTENANCE:
isMaintenanceInterrupt = true;
break;
+ case TIMER_IRQID(CURRENT_TIMER):
+ timerInterruptHandler();
+ break;
default:
isGuestInterrupt = irqId >= 16;
break;
diff --git a/thermosphere/src/irq.h b/thermosphere/src/irq.h
index a80997d9f..341a891c9 100644
--- a/thermosphere/src/irq.h
+++ b/thermosphere/src/irq.h
@@ -77,5 +77,15 @@ static inline bool irqIsGuest(u16 id)
return false;
}
- return id != GIC_IRQID_MAINTENANCE && id != GIC_IRQID_HYP_TIMER;
+ bool ret = id != GIC_IRQID_MAINTENANCE && id != GIC_IRQID_NS_PHYS_HYP_TIMER;
+#if GIC_IRQID_NS_VIRT_HYP_TIMER != GIC_IRQID_SPURIOUS
+ ret = ret && id != GIC_IRQID_NS_VIRT_HYP_TIMER;
+#endif
+#if GIC_IRQID_SEC_PHYS_HYP_TIMER != GIC_IRQID_SPURIOUS
+ ret = ret && id != GIC_IRQID_SEC_PHYS_HYP_TIMER;
+#endif
+#if GIC_IRQID_SEC_VIRT_HYP_TIMER != GIC_IRQID_SPURIOUS
+ ret = ret && id != GIC_IRQID_SEC_VIRT_HYP_TIMER;
+#endif
+ return ret;
}
diff --git a/thermosphere/src/main.c b/thermosphere/src/main.c
index 5f055b415..244569d75 100644
--- a/thermosphere/src/main.c
+++ b/thermosphere/src/main.c
@@ -11,7 +11,7 @@
#include "single_step.h"
#include "breakpoints.h"
#include "watchpoints.h"
-
+#include "timer.h"
#include "irq.h"
extern const u8 __start__[];
@@ -41,6 +41,7 @@ void thermosphereMain(ExceptionStackFrame *frame)
{
enableTraps();
enableBreakpointsAndWatchpoints();
+ timerInit();
initIrq();
if (currentCoreCtx->isBootCore) {
diff --git a/thermosphere/src/platform/qemu/interrupt_config.h b/thermosphere/src/platform/qemu/interrupt_config.h
index 92ab936e5..9e301ceb1 100644
--- a/thermosphere/src/platform/qemu/interrupt_config.h
+++ b/thermosphere/src/platform/qemu/interrupt_config.h
@@ -23,13 +23,18 @@
#define GIC_IRQID_PMU 23
#define GIC_IRQID_MAINTENANCE 25
-#define GIC_IRQID_HYP_TIMER 26
-#define GIC_IRQID_VIRT_TIMER 27
+#define GIC_IRQID_NS_PHYS_HYP_TIMER 26
+#define GIC_IRQID_NS_VIRT_TIMER 27
//#define GIC_IRQID_LEGACY_NFIQ 28 not defined?
#define GIC_IRQID_SEC_PHYS_TIMER 29
#define GIC_IRQID_NS_PHYS_TIMER 30
//#define GIC_IRQID_LEGACY_NIRQ 31 not defined?
+
+#define GIC_IRQID_NS_VIRT_HYP_TIMER GIC_IRQID_SPURIOUS // SBSA: 28. Unimplemented
+#define GIC_IRQID_SEC_PHYS_HYP_TIMER GIC_IRQID_SPURIOUS // SBSA: 20. Unimplemented
+#define GIC_IRQID_SEC_VIRT_HYP_TIMER GIC_IRQID_SPURIOUS // SBSA: 19. Unimplemented
+
static inline void initGicV2Pointers(ArmGicV2 *gic)
{
gic->gicd = (volatile ArmGicV2Distributor *)0x08000000ull;
diff --git a/thermosphere/src/platform/tegra/interrupt_config.h b/thermosphere/src/platform/tegra/interrupt_config.h
index 562b88abb..1b3b48789 100644
--- a/thermosphere/src/platform/tegra/interrupt_config.h
+++ b/thermosphere/src/platform/tegra/interrupt_config.h
@@ -21,14 +21,17 @@
// For both guest and host
#define MAX_NUM_REGISTERED_INTERRUPTS 512
-#define GIC_IRQID_MAINTENANCE 25
-#define GIC_IRQID_HYP_TIMER 26
-#define GIC_IRQID_VIRT_TIMER 27
-#define GIC_IRQID_LEGACY_NFIQ 28
-#define GIC_IRQID_SEC_PHYS_TIMER 29
-#define GIC_IRQID_NS_PHYS_TIMER 30
-#define GIC_IRQID_LEGACY_NIRQ 31
+#define GIC_IRQID_MAINTENANCE 25
+#define GIC_IRQID_NS_PHYS_HYP_TIMER 26
+#define GIC_IRQID_NS_VIRT_TIMER 27
+#define GIC_IRQID_LEGACY_NFIQ 28
+#define GIC_IRQID_SEC_PHYS_TIMER 29
+#define GIC_IRQID_NS_PHYS_TIMER 30
+#define GIC_IRQID_LEGACY_NIRQ 31
+#define GIC_IRQID_NS_VIRT_HYP_TIMER GIC_IRQID_SPURIOUS // SBSA: 28. Unimplemented
+#define GIC_IRQID_SEC_PHYS_HYP_TIMER GIC_IRQID_SPURIOUS // SBSA: 20. Unimplemented
+#define GIC_IRQID_SEC_VIRT_HYP_TIMER GIC_IRQID_SPURIOUS // SBSA: 19. Unimplemented
static inline void initGicV2Pointers(ArmGicV2 *gic)
{
diff --git a/thermosphere/src/sysreg.h b/thermosphere/src/sysreg.h
index c079d1e2b..128211e05 100644
--- a/thermosphere/src/sysreg.h
+++ b/thermosphere/src/sysreg.h
@@ -428,12 +428,12 @@
#define GET_SYSREG(r) ({\
u64 __val; \
- asm volatile("mrs %0, " STRINGIZE(r) : "=r" (__val)); \
+ asm volatile("mrs %0, " STRINGIZE(r) : "=r" (__val)); \
__val; \
})
-#define SET_SYSREG(reg, val) do { u64 temp_reg = (val); __asm__ __volatile__ ("msr " #reg ", %0" :: "r"(temp_reg) : "memory"); } while(false)
-#define SET_SYSREG_IMM(reg, imm) do { __asm__ __volatile__ ("msr " #reg ", %0" :: "I"(imm) : "memory"); } while(false)
+#define SET_SYSREG(reg, val) do { u64 temp_reg = (val); __asm__ __volatile__ ("msr " STRINGIZE(reg) ", %0" :: "r"(temp_reg) : "memory"); } while(false)
+#define SET_SYSREG_IMM(reg, imm) do { __asm__ __volatile__ ("msr " STRINGIZE(reg) ", %0" :: "I"(imm) : "memory"); } while(false)
#define SYSREG_OP1_AARCH32_AUTO 0
#define SYSREG_OP1_AARCH64_EL1 0
diff --git a/thermosphere/src/timer.c b/thermosphere/src/timer.c
new file mode 100644
index 000000000..5fd711357
--- /dev/null
+++ b/thermosphere/src/timer.c
@@ -0,0 +1,38 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+#include "timer.h"
+#include "irq.h"
+
+u64 g_timerFreq = 0;
+
+void timerInit(void)
+{
+ timerSetInterruptStatus(false, false);
+ if (currentCoreCtx->isBootCore) {
+ g_timerFreq = GET_SYSREG(cntfrq_el0);
+ }
+}
+
+void timerInterruptHandler(void)
+{
+ // Disable timer interrupts until reprogrammed
+ timerSetInterruptStatus(false, false);
+
+ // For fun
+ DEBUG("EL2 [core %d]: Timer interrupt at %lums\n", (int)currentCoreCtx->coreId, timerGetSystemTimeMs());
+ timerSetTimeoutMs(1000);
+}
diff --git a/thermosphere/src/timer.h b/thermosphere/src/timer.h
new file mode 100644
index 000000000..c18c28543
--- /dev/null
+++ b/thermosphere/src/timer.h
@@ -0,0 +1,91 @@
+/*
+ * Copyright (c) 2019 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see .
+ */
+
+#pragma once
+
+#include "utils.h"
+#include "sysreg.h"
+#include "platform/interrupt_config.h"
+
+#define SECTONSECS 1000000000ull
+#define SECTOMSECS 1000ull
+
+// All generic timers possibly defined in the Arm architecture:
+// (suffix, time source suffix, level, irqid)
+#define NS_PHYS_TIMER (p, p, 0, GIC_IRQID_NS_PHYS_TIMER)
+#define NS_VIRT_TIMER (v, v, 0, GIC_IRQID_NS_VIRT_TIMER)
+#define NS_PHYS_HYP_TIMER (hp, p, 2, GIC_IRQID_NS_PHYS_HYP_TIMER)
+#define NS_VIRT_HYP_TIMER (hv, v, 2, GIC_IRQID_NS_VIRT_HYP_TIMER)
+#define SEC_PHYS_TIMER (ps, p, 1, GIC_IRQID_SEC_PHYS_TIMER)
+#define SEC_PHYS_HYP_TIMER (hps, p, 2, GIC_IRQID_SEC_PHYS_HYP_TIMER)
+#define SEC_VIRT_HYP_TIMER (hvs, v, 2, GIC_IRQID_SEC_VIRT_HYP_TIMER)
+
+#define TIMER_IRQID_FIELDS(ign, ign2, ign3, id) id
+#define TIMER_COUNTER_REG_FIELDS(ign, ts, ign2, ign3) cnt##ts##ct_el0
+#define TIMER_CTL_REG_FIELDS(t, ign, el, ign2) cnt##t##_ctl_el##el
+#define TIMER_CVAL_REG_FIELDS(t, ign, el, ign2) cnt##t##_cval_el##el
+#define TIMER_TVAL_REG_FIELDS(t, ign, el, ign2) cnt##t##_tval_el##el
+
+#define TIMER_IRQID(name) EVAL(TIMER_IRQID_FIELDS name)
+#define TIMER_COUNTER_REG(name) EVAL(TIMER_COUNTER_REG_FIELDS name)
+#define TIMER_CTL_REG(name) EVAL(TIMER_CTL_REG_FIELDS name)
+#define TIMER_CVAL_REG(name) EVAL(TIMER_CVAL_REG_FIELDS name)
+#define TIMER_TVAL_REG(name) EVAL(TIMER_TVAL_REG_FIELDS name)
+
+#define CURRENT_TIMER NS_PHYS_HYP_TIMER
+
+extern u64 g_timerFreq;
+
+void timerInit(void);
+void timerInterruptHandler(void);
+
+static inline u64 timerGetSystemTick(void)
+{
+ return GET_SYSREG(TIMER_COUNTER_REG(CURRENT_TIMER));
+}
+
+static inline u64 timerGetSystemTimeNs(void)
+{
+ return timerGetSystemTick() * SECTONSECS / g_timerFreq;
+}
+
+static inline u64 timerGetSystemTimeMs(void)
+{
+ return timerGetSystemTick() * SECTOMSECS / g_timerFreq;
+}
+
+static inline void timerSetInterruptStatus(bool enabled, bool masked)
+{
+ u32 ebit = enabled ? BIT(0) : 0;
+ u32 mbit = masked ? BIT(1) : 0;
+ SET_SYSREG(TIMER_CTL_REG(CURRENT_TIMER), mbit | ebit);
+}
+
+static inline void timerSetTimeoutTicks(u64 ticks)
+{
+ SET_SYSREG(TIMER_CVAL_REG(CURRENT_TIMER), timerGetSystemTick() + ticks);
+ timerSetInterruptStatus(true, false);
+}
+
+static inline void timerSetTimeoutNs(u64 ns)
+{
+ timerSetTimeoutTicks(ns * g_timerFreq / SECTONSECS);
+}
+
+static inline void timerSetTimeoutMs(u64 ms)
+{
+ timerSetTimeoutTicks(ms * g_timerFreq / SECTOMSECS);
+}
diff --git a/thermosphere/src/utils.h b/thermosphere/src/utils.h
index 8c7543abc..98655d30a 100644
--- a/thermosphere/src/utils.h
+++ b/thermosphere/src/utils.h
@@ -46,7 +46,7 @@
static inline u##sz __##op##sz(u##sz n)\
{\
u##sz res;\
- __asm__ __volatile__ (#op " %" #regalloc "[res], %" #regalloc "[n]" : [res] "=r" (res) : [n] "r" (n));\
+ __asm__ __volatile__ (STRINGIZE(op) " %" STRINGIZE(regalloc) "[res], %" STRINGIZE(regalloc) "[n]" : [res] "=r" (res) : [n] "r" (n));\
return res;\
}