kern: implement init through InterruptTaskManager.Initialize()

libraries/libmesosphere/include/mesosphere/arch/arm64/kern_cpu.hpp

@@ -59,6 +59,12 @@ namespace ams::kern::arm64::cpu {
         EnsureInstructionConsistency();
     }
 
+    ALWAYS_INLINE void SwitchProcess(u64 ttbr, u32 proc_id) {
+        SetTtbr0El1(ttbr);
+        ContextIdRegisterAccessor(0).SetProcId(proc_id).Store();
+        InstructionMemoryBarrier();
+    }
+
     /* Helper for address access. */
     ALWAYS_INLINE bool GetPhysicalAddressWritable(KPhysicalAddress *out, KVirtualAddress addr, bool privileged = false) {
         const uintptr_t va = GetInteger(addr);

libraries/libmesosphere/include/mesosphere/arch/arm64/kern_k_page_table.hpp

@@ -34,6 +34,11 @@ namespace ams::kern::arm64 {
 
             static NOINLINE void Initialize(s32 core_id);
 
+            ALWAYS_INLINE void Activate(u32 proc_id) {
+                cpu::DataSynchronizationBarrier();
+                cpu::SwitchProcess(this->ttbr, proc_id);
+            }
+
             NOINLINE Result InitializeForKernel(void *table, KVirtualAddress start, KVirtualAddress end);
             Result Finalize();
     };

libraries/libmesosphere/include/mesosphere/arch/arm64/kern_k_supervisor_page_table.hpp

@@ -28,6 +28,7 @@ namespace ams::kern::arm64 {
             constexpr KSupervisorPageTable() : page_table(), ttbr0() { /* ... */ }
 
             NOINLINE void Initialize(s32 core_id);
+            NOINLINE void Activate();
             void Finalize(s32 core_id);
     };
 

libraries/libmesosphere/include/mesosphere/kern_k_interrupt_task_manager.hpp

@@ -39,10 +39,16 @@ namespace ams::kern {
         private:
             TaskQueue task_queue;
             KThread *thread;
+        private:
+            static void ThreadFunction(uintptr_t arg);
+            void ThreadFunctionImpl();
         public:
             constexpr KInterruptTaskManager() : task_queue(), thread(nullptr) { /* ... */ }
 
-            constexpr ALWAYS_INLINE KThread *GetThread() const { return this->thread; }
+            constexpr KThread *GetThread() const { return this->thread; }
+
+            NOINLINE void Initialize();
+            void EnqueueTask(KInterruptTask *task);
 
             /* TODO: Actually implement KInterruptTaskManager. This is a placeholder. */
     };

libraries/libmesosphere/source/arch/arm64/kern_k_interrupt_manager.cpp

@@ -85,7 +85,7 @@ namespace ams::kern::arm64 {
 
         /* If the task isn't the dummy task, we should add it to the queue. */
         if (task != GetDummyInterruptTask()) {
-            MESOSPHERE_TODO("Kernel::GetInterruptTaskManager().Enqueue(task);");
+            Kernel::GetInterruptTaskManager().EnqueueTask(task);
         }
 
         return true;

libraries/libmesosphere/source/arch/arm64/kern_k_supervisor_page_table.cpp

@@ -39,6 +39,14 @@ namespace ams::kern::arm64 {
         }
     }
 
+    void KSupervisorPageTable::Activate() {
+        /* Initialize, using process id = 0xFFFFFFFF */
+        this->page_table.Initialize(0xFFFFFFFF);
+
+        /* Invalidate entire TLB. */
+        cpu::InvalidateEntireTlb();
+    }
+
     void KSupervisorPageTable::Finalize(s32 core_id) {
         MESOSPHERE_TODO_IMPLEMENT();
     }

libraries/libmesosphere/source/kern_k_interrupt_task_manager.cpp

@@ -18,6 +18,10 @@
 namespace ams::kern {
 
     void KInterruptTaskManager::TaskQueue::Enqueue(KInterruptTask *task) {
+        MESOSPHERE_ASSERT(task->GetNextTask() == nullptr);
+        MESOSPHERE_ASSERT(task != this->head);
+        MESOSPHERE_ASSERT(task != this->tail);
+
         /* Insert the task into the queue. */
         if (this->tail != nullptr) {
             this->tail->SetNextTask(task);
@@ -29,6 +33,10 @@ namespace ams::kern {
     }
 
     void KInterruptTaskManager::TaskQueue::Dequeue() {
+        MESOSPHERE_ASSERT(this->head != nullptr);
+        MESOSPHERE_ASSERT(this->tail != nullptr);
+
+        /* Pop the task from the front of the queue. */
         if (this->head == this->tail) {
             this->head = nullptr;
             this->tail = nullptr;
@@ -37,4 +45,51 @@ namespace ams::kern {
         }
     }
 
+    void KInterruptTaskManager::ThreadFunction(uintptr_t arg) {
+        reinterpret_cast<KInterruptTaskManager *>(arg)->ThreadFunctionImpl();
+    }
+
+    void KInterruptTaskManager::ThreadFunctionImpl() {
+        MESOSPHERE_ASSERT_THIS();
+
+        while (true) {
+            /* Get a task. */
+            KInterruptTask *task = nullptr;
+            {
+                KScopedInterruptDisable di;
+
+                task = this->task_queue.GetHead();
+                if (task == nullptr) {
+                    this->thread->SetState(KThread::ThreadState_Waiting);
+                    continue;
+                }
+
+                this->task_queue.Dequeue();
+            }
+
+            /* Do the task. */
+            task->DoTask();
+        }
+    }
+
+    void KInterruptTaskManager::Initialize() {
+        /* Reserve a thread from the system limit. */
+        MESOSPHERE_ABORT_UNLESS(Kernel::GetSystemResourceLimit().Reserve(ams::svc::LimitableResource_ThreadCountMax, 1));
+
+        /* Create and initialize the thread. */
+        this->thread = KThread::Create();
+        MESOSPHERE_ABORT_UNLESS(this->thread != nullptr);
+        MESOSPHERE_R_ABORT_UNLESS(KThread::InitializeHighPriorityThread(this->thread, ThreadFunction, reinterpret_cast<uintptr_t>(this)));
+        KThread::Register(this->thread);
+
+        /* Run the thread. */
+        this->thread->Run();
+    }
+
+    void KInterruptTaskManager::EnqueueTask(KInterruptTask *task) {
+        MESOSPHERE_ASSERT(!KInterruptManager::AreInterruptsEnabled());
+
+        MESOSPHERE_TODO_IMPLEMENT();
+    }
+
 }

libraries/libmesosphere/source/kern_main.cpp

@@ -78,9 +78,9 @@ namespace ams::kern {
             Kernel::GetKernelPageTable().Initialize(core_id);
         });
 
-        /* Set ttbr0 for each core. */
+        /* Activate the supervisor page table for each core. */
         DoOnEachCoreInOrder(core_id, [=]() ALWAYS_INLINE_LAMBDA {
-            MESOSPHERE_TODO("SetTtbr0();");
+            Kernel::GetKernelPageTable().Activate();
         });
 
         /* NOTE: Kernel calls on each core a nullsub here on retail kernel. */
@@ -89,7 +89,7 @@ namespace ams::kern {
         DoOnEachCoreInOrder(core_id, [=]() ALWAYS_INLINE_LAMBDA {
             KThread::Register(std::addressof(Kernel::GetMainThread(core_id)));
             KThread::Register(std::addressof(Kernel::GetIdleThread(core_id)));
-            MESOSPHERE_TODO("Kernel::GetInterruptTaskManager().Initialize();");
+            Kernel::GetInterruptTaskManager().Initialize();
         });
 
         /* Activate the scheduler and enable interrupts. */
@@ -99,7 +99,7 @@ namespace ams::kern {
         });
 
         /* Initialize cpu interrupt threads. */
-        /* TODO cpu::InitializeInterruptThreads(core_id); */
+        MESOSPHERE_TODO("cpu::InitializeInterruptThreads(core_id);");
 
         /* Initialize the DPC manager. */
         KDpcManager::Initialize();