Implement CPU state init, fix bug in cpu_context restore

exosphere/src/bootup.c

@@ -19,6 +19,7 @@
 #include "interrupt.h"
 #include "cpu_context.h"
 #include "actmon.h"
+#include "syscrt0.h"
 
 static bool g_has_booted_up = false;
 
@@ -166,3 +167,34 @@ void bootup_misc_mmio(void) {
 void setup_4x_mmio(void) {
     /* TODO */
 }
+
+#define SET_SYSREG(reg, val) do { temp_reg = val; __asm__ __volatile__ ("msr " #reg ", %0" :: "r"(temp_reg) : "memory"); } while(false)
+
+void setup_current_core_state(void) {
+    uint64_t temp_reg;
+    
+    /* Setup system registers. */
+    SET_SYSREG(actlr_el3, 0x73ull);
+    SET_SYSREG(actlr_el2, 0x73ull);
+    SET_SYSREG(hcr_el2, 0x80000000ull);
+    SET_SYSREG(dacr32_el2, 0xFFFFFFFFull);
+    SET_SYSREG(sctlr_el1, 0xC50838ull);
+    SET_SYSREG(sctlr_el2, 0x30C50838ull);
+    
+    do { __asm__ __volatile__ ("isb"); } while (false);
+    
+    SET_SYSREG(cntfrq_el0, MAKE_SYSCRT0_REG(0x20)); /* TODO: Reg name. */
+    SET_SYSREG(cnthctl_el2, 3ull);
+
+    do { __asm__ __volatile__ ("isb"); } while (false);
+
+    /* Setup Interrupts, flow. */
+    flow_clear_csr0_and_events(get_core_id());
+    intr_initialize_gic();
+    intr_set_priority(INTERRUPT_ID_1C, 0);
+    intr_set_group(INTERRUPT_ID_1C, 0);
+    intr_set_enabled(INTERRUPT_ID_1C, 1);
+
+    /* Restore current core context. */
+    restore_current_core_context();
+}

exosphere/src/bootup.h

@@ -7,4 +7,6 @@ void bootup_misc_mmio(void);
 
 void setup_4x_mmio(void);
 
+void setup_current_core_state(void);
+
 #endif

exosphere/src/cpu_context.c

@@ -16,8 +16,8 @@
 
 #define RESTORE_SYSREG64(reg) do { temp_reg = g_cpu_contexts[current_core].reg; __asm__ __volatile__ ("msr " #reg ", %0" :: "r"(temp_reg) : "memory"); } while(false)
 #define RESTORE_SYSREG32(reg) RESTORE_SYSREG64(reg)
-#define RESTORE_BP_REG(i, _) SAVE_SYSREG64(DBGBVR##i##_EL1); SAVE_SYSREG64(DBGBCR##i##_EL1);
+#define RESTORE_BP_REG(i, _) RESTORE_SYSREG64(DBGBVR##i##_EL1); RESTORE_SYSREG64(DBGBCR##i##_EL1);
-#define RESTORE_WP_REG(i, _) SAVE_SYSREG64(DBGBVR##i##_EL1); SAVE_SYSREG64(DBGBCR##i##_EL1);
+#define RESTORE_WP_REG(i, _) RESTORE_SYSREG64(DBGBVR##i##_EL1); RESTORE_SYSREG64(DBGBCR##i##_EL1);
 
 static saved_cpu_context_t g_cpu_contexts[NUM_CPU_CORES] = {0};
 

exosphere/src/interrupt.h

@@ -10,6 +10,7 @@
 #define MAX_REGISTERED_INTERRUPTS 4
 #define INTERRUPT_ID_SECURITY_ENGINE 0x5A
 #define INTERRUPT_ID_ACTIVITY_MONITOR_4X 0x4D
+#define INTERRUPT_ID_1C 0x1C
 #define INTERRUPT_ID_USER_SECURITY_ENGINE 0x2C
 
 static inline uintptr_t get_gicd_base(void) {

exosphere/src/package2.c

@@ -380,7 +380,7 @@ void load_package2(coldboot_crt0_reloc_list_t *reloc_list) {
 
     bootup_misc_mmio();
 
-    /* TODO: initalize cpu context */
+    setup_current_core_state();
 
     /* TODO: Read and save BOOTREASON stored by NX_BOOTLOADER at 0x1F009FE00 */
     

exosphere/src/syscrt0.h

@@ -0,0 +1,17 @@
+#ifndef EXOSPHERE_SYSCRT0_H
+#define EXOSPHERE_SYSCRT0_H
+
+#include <stdint.h>
+
+#include "memory_map.h"
+
+/* Exosphere driver for the Tegra X1 SYSCRT0 Registers. */
+
+#define SYSCRT0_BASE  (MMIO_GET_DEVICE_ADDRESS(MMIO_DEVID_SYSCTR0))
+
+
+#define MAKE_SYSCRT0_REG(n) (*((volatile uint32_t *)(SYSCRT0_BASE + n)))
+
+
+
+#endif