10 files changed, 275 insertions, 76 deletions

diff --git a/src/core/arm/dyncom/arm_dyncom_interpreter.cpp b/src/core/arm/dyncom/arm_dyncom_interpreter.cpp
index 68012bffd..84b4a38f0 100644
--- a/src/core/arm/dyncom/arm_dyncom_interpreter.cpp
+++ b/src/core/arm/dyncom/arm_dyncom_interpreter.cpp
@@ -1266,6 +1266,13 @@ typedef struct _smla_inst {
         unsigned int Rn;
 } smla_inst;
 
+typedef struct umaal_inst {
+        unsigned int Rn;
+        unsigned int Rm;
+        unsigned int RdHi;
+        unsigned int RdLo;
+} umaal_inst;
+
 typedef struct _umlal_inst {
         unsigned int S;
         unsigned int Rm;
@@ -3010,7 +3017,26 @@ ARM_INST_PTR INTERPRETER_TRANSLATE(uhaddsubx)(unsigned int inst, int index) { UN
 ARM_INST_PTR INTERPRETER_TRANSLATE(uhsub16)(unsigned int inst, int index)   { UNIMPLEMENTED_INSTRUCTION("UHSUB16"); }
 ARM_INST_PTR INTERPRETER_TRANSLATE(uhsub8)(unsigned int inst, int index)    { UNIMPLEMENTED_INSTRUCTION("UHSUB8"); }
 ARM_INST_PTR INTERPRETER_TRANSLATE(uhsubaddx)(unsigned int inst, int index) { UNIMPLEMENTED_INSTRUCTION("UHSUBADDX"); }
-ARM_INST_PTR INTERPRETER_TRANSLATE(umaal)(unsigned int inst, int index)     { UNIMPLEMENTED_INSTRUCTION("UMAAL"); }
+ARM_INST_PTR INTERPRETER_TRANSLATE(umaal)(unsigned int inst, int index)
+{
+        arm_inst* const inst_base = (arm_inst*)AllocBuffer(sizeof(arm_inst) + sizeof(umaal_inst));
+        umaal_inst* const inst_cream = (umaal_inst*)inst_base->component;
+
+        inst_base->cond     = BITS(inst, 28, 31);
+        inst_base->idx      = index;
+        inst_base->br       = NON_BRANCH;
+        inst_base->load_r15 = 0;
+
+        inst_cream->Rm   = BITS(inst, 8, 11);
+        inst_cream->Rn   = BITS(inst, 0, 3);
+        inst_cream->RdLo = BITS(inst, 12, 15);
+        inst_cream->RdHi = BITS(inst, 16, 19);
+
+        if (CHECK_RM || CHECK_RN)
+                inst_base->load_r15 = 1;
+
+        return inst_base;
+}
 ARM_INST_PTR INTERPRETER_TRANSLATE(umlal)(unsigned int inst, int index)
 {
         arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(umlal_inst));
@@ -6374,6 +6400,26 @@ unsigned InterpreterMainLoop(ARMul_State* state)
         UHSUB8_INST:
         UHSUBADDX_INST:
         UMAAL_INST:
+        {
+                INC_ICOUNTER;
+                if (inst_base->cond == 0xE || CondPassed(cpu, inst_base->cond)) {
+                        umaal_inst* const inst_cream = (umaal_inst*)inst_base->component;
+
+                        const u32 rm = RM;
+                        const u32 rn = RN;
+                        const u32 rd_lo = RDLO;
+                        const u32 rd_hi = RDHI;
+
+                        const u64 result = (rm * rn) + rd_lo + rd_hi;
+
+                        RDLO = (result & 0xFFFFFFFF);
+                        RDHI = ((result >> 32) & 0xFFFFFFFF);
+                }
+                cpu->Reg[15] += GET_INST_SIZE(cpu);
+                INC_PC(sizeof(umaal_inst));
+                FETCH_INST;
+                GOTO_NEXT_INST;
+        }
         UMLAL_INST:
         {
                 INC_ICOUNTER;
diff --git a/src/core/arm/interpreter/armemu.cpp b/src/core/arm/interpreter/armemu.cpp
index 63cfd03c6..610e04f10 100644
--- a/src/core/arm/interpreter/armemu.cpp
+++ b/src/core/arm/interpreter/armemu.cpp
@@ -5824,9 +5824,9 @@ L_stm_s_takeabort:
         case 0x3f:
             printf ("Unhandled v6 insn: rbit\n");
             break;
-        case 0x61: // SSUB16, SADD16, SSAX, and SASX
-            if ((instr & 0xFF0) == 0xf70 || (instr & 0xFF0) == 0xf10 ||
-                (instr & 0xFF0) == 0xf50 || (instr & 0xFF0) == 0xf30)
+        case 0x61: // SADD16, SASX, SSAX, and SSUB16
+            if ((instr & 0xFF0) == 0xf10 || (instr & 0xFF0) == 0xf30 ||
+                (instr & 0xFF0) == 0xf50 || (instr & 0xFF0) == 0xf70)
             {
                 const u8 rd_idx = BITS(12, 15);
                 const u8 rm_idx = BITS(0, 3);
@@ -5839,25 +5839,25 @@ L_stm_s_takeabort:
                 s32 lo_result;
                 s32 hi_result;
 
-                // SSUB16
-                if ((instr & 0xFF0) == 0xf70) {
-                    lo_result = (rn_lo - rm_lo);
-                    hi_result = (rn_hi - rm_hi);
-                }
                 // SADD16
-                else if ((instr & 0xFF0) == 0xf10) {
+                if ((instr & 0xFF0) == 0xf10) {
                     lo_result = (rn_lo + rm_lo);
                     hi_result = (rn_hi + rm_hi);
                 }
+                // SASX
+                else if ((instr & 0xFF0) == 0xf30) {
+                    lo_result = (rn_lo - rm_hi);
+                    hi_result = (rn_hi + rm_lo);
+                }
                 // SSAX
                 else if ((instr & 0xFF0) == 0xf50) {
                     lo_result = (rn_lo + rm_hi);
                     hi_result = (rn_hi - rm_lo);
                 }
-                // SASX
+                // SSUB16
                 else {
-                    lo_result = (rn_lo - rm_hi);
-                    hi_result = (rn_hi + rm_lo);
+                    lo_result = (rn_lo - rm_lo);
+                    hi_result = (rn_hi - rm_hi);
                 }
 
                 state->Reg[rd_idx] = (lo_result & 0xFFFF) | ((hi_result & 0xFFFF) << 16);
@@ -5878,12 +5878,87 @@ L_stm_s_takeabort:
                     state->Cpsr &= ~(1 << 19);
                 }
                 return 1;
-            } else {
-                printf("Unhandled v6 insn: %08x", BITS(20, 27));
+            }
+            // SADD8/SSUB8
+            else  if ((instr & 0xFF0) == 0xf90 || (instr & 0xFF0) == 0xff0)
+            {
+                const u8 rd_idx = BITS(12, 15);
+                const u8 rm_idx = BITS(0, 3);
+                const u8 rn_idx = BITS(16, 19);
+                const u32 rm_val = state->Reg[rm_idx];
+                const u32 rn_val = state->Reg[rn_idx];
+
+                u8 lo_val1;
+                u8 lo_val2;
+                u8 hi_val1;
+                u8 hi_val2;
+
+                // SADD8
+                if ((instr & 0xFF0) == 0xf90) {
+                    lo_val1 = (u8)((rn_val & 0xFF) + (rm_val & 0xFF));
+                    lo_val2 = (u8)(((rn_val >> 8) & 0xFF) + ((rm_val >> 8) & 0xFF));
+                    hi_val1 = (u8)(((rn_val >> 16) & 0xFF) + ((rm_val >> 16) & 0xFF));
+                    hi_val2 = (u8)(((rn_val >> 24) & 0xFF) + ((rm_val >> 24) & 0xFF));
+
+                    if (lo_val1 & 0x80)
+                        state->Cpsr |= (1 << 16);
+                    else
+                        state->Cpsr &= ~(1 << 16);
+
+                    if (lo_val2 & 0x80)
+                        state->Cpsr |= (1 << 17);
+                    else
+                        state->Cpsr &= ~(1 << 17);
+
+                    if (hi_val1 & 0x80)
+                        state->Cpsr |= (1 << 18);
+                    else
+                        state->Cpsr &= ~(1 << 18);
+
+                    if (hi_val2 & 0x80)
+                        state->Cpsr |= (1 << 19);
+                    else
+                        state->Cpsr &= ~(1 << 19);
+                }
+                // SSUB8
+                else {
+                    lo_val1 = (u8)((rn_val & 0xFF) - (rm_val & 0xFF));
+                    lo_val2 = (u8)(((rn_val >> 8) & 0xFF) - ((rm_val >> 8) & 0xFF));
+                    hi_val1 = (u8)(((rn_val >> 16) & 0xFF) - ((rm_val >> 16) & 0xFF));
+                    hi_val2 = (u8)(((rn_val >> 24) & 0xFF) - ((rm_val >> 24) & 0xFF));
+
+                    if (!(lo_val1 & 0x80))
+                        state->Cpsr |= (1 << 16);
+                    else
+                        state->Cpsr &= ~(1 << 16);
+
+                    if (!(lo_val2 & 0x80))
+                        state->Cpsr |= (1 << 17);
+                    else
+                        state->Cpsr &= ~(1 << 17);
+
+                    if (!(hi_val1 & 0x80))
+                        state->Cpsr |= (1 << 18);
+                    else
+                        state->Cpsr &= ~(1 << 18);
+
+                    if (!(hi_val2 & 0x80))
+                        state->Cpsr |= (1 << 19);
+                    else
+                        state->Cpsr &= ~(1 << 19);
+                }
+
+                state->Reg[rd_idx] = (lo_val1 | lo_val2 << 8 | hi_val1 << 16 | hi_val2 << 24);
+                return 1;
+            }
+            else {
+                printf("Unhandled v6 insn: %08x", instr);
             }
             break;
-        case 0x62: // QSUB16 and QADD16
-            if ((instr & 0xFF0) == 0xf70 || (instr & 0xFF0) == 0xf10) {
+        case 0x62: // QADD16, QASX, QSAX, and QSUB16
+            if ((instr & 0xFF0) == 0xf10 || (instr & 0xFF0) == 0xf30 ||
+                (instr & 0xFF0) == 0xf50 || (instr & 0xFF0) == 0xf70)
+            {
                 const u8 rd_idx = BITS(12, 15);
                 const u8 rn_idx = BITS(16, 19);
                 const u8 rm_idx = BITS(0, 3);
@@ -5895,15 +5970,26 @@ L_stm_s_takeabort:
                 s32 lo_result;
                 s32 hi_result;
 
+                // QADD16
+                if ((instr & 0xFF0) == 0xf10) {
+                    lo_result = (rn_lo + rm_lo);
+                    hi_result = (rn_hi + rm_hi);
+                }
+                // QASX
+                else if ((instr & 0xFF0) == 0xf30) {
+                    lo_result = (rn_lo - rm_hi);
+                    hi_result = (rn_hi + rm_lo);
+                }
+                // QSAX
+                else if ((instr & 0xFF0) == 0xf50) {
+                    lo_result = (rn_lo + rm_hi);
+                    hi_result = (rn_hi - rm_lo);
+                }
                 // QSUB16
-                if ((instr & 0xFF0) == 0xf70) {
+                else {
                     lo_result = (rn_lo - rm_lo);
                     hi_result = (rn_hi - rm_hi);
                 }
-                else { // QADD16
-                    lo_result = (rn_lo + rm_lo);
-                    hi_result = (rn_hi + rm_hi);
-                }
 
                 if (lo_result > 0x7FFF)
                     lo_result = 0x7FFF;
@@ -6078,22 +6164,28 @@ L_stm_s_takeabort:
                                         //ichfly
                                         //SSAT16
                                 {
-                                        u8 tar = BITS(12, 15);
-                                        u8 src = BITS(0, 3);
-                                        u8 val = BITS(16, 19) + 1;
-                                        s16 a1 = (state->Reg[src]);
-                                        s16 a2 = (state->Reg[src] >> 0x10);
-                                        s16 min = (s16)(0x8000 >> (16 - val));
-                                        s16 max = 0x7FFF >> (16 - val);
-                                        if (min > a1) a1 = min;
-                                        if (max < a1) a1 = max;
-                                        if (min > a2) a2 = min;
-                                        if (max < a2) a2 = max;
-                                        u32 temp2 = ((u32)(a2)) << 0x10;
-                                        state->Reg[tar] = (a1 & 0xFFFF) | (temp2);
+                                        const u8 rd_idx = BITS(12, 15);
+                                        const u8 rn_idx = BITS(0, 3);
+                                        const u8 num_bits = BITS(16, 19) + 1;
+                                        const s16 min = -(0x8000 >> (16 - num_bits));
+                                        const s16 max = (0x7FFF >> (16 - num_bits));
+                                        s16 rn_lo = (state->Reg[rn_idx]);
+                                        s16 rn_hi = (state->Reg[rn_idx] >> 16);
+
+                                        if (rn_lo > max)
+                                                rn_lo = max;
+                                        else if (rn_lo < min)
+                                                rn_lo = min;
+
+                                        if (rn_hi > max)
+                                                rn_hi = max;
+                                        else if (rn_hi < min)
+                                                rn_hi = min;
+
+                                        state->Reg[rd_idx] = (rn_lo & 0xFFFF) | ((rn_hi & 0xFFFF) << 16);
+                                        return 1;
                                 }
 
-                                return 1;
                                 default:
                                         break;
                         }
@@ -6314,11 +6406,14 @@ L_stm_s_takeabort:
                 }
         case 0x70:
             // ichfly
-            // SMUAD, SMUSD, SMLAD
-            if ((instr & 0xf0d0) == 0xf010 || (instr & 0xf0d0) == 0xf050 || (instr & 0xd0) == 0x10) {
+            // SMUAD, SMUSD, SMLAD, and SMLSD
+            if ((instr & 0xf0d0) == 0xf010 || (instr & 0xf0d0) == 0xf050 ||
+                (instr & 0xd0) == 0x10     || (instr & 0xd0) == 0x50)
+            {
                 const u8 rd_idx = BITS(16, 19);
                 const u8 rn_idx = BITS(0, 3);
                 const u8 rm_idx = BITS(8, 11);
+                const u8 ra_idx = BITS(12, 15);
                 const bool do_swap = (BIT(5) == 1);
 
                 u32 rm_val = state->Reg[rm_idx];
@@ -6341,13 +6436,14 @@ L_stm_s_takeabort:
                     state->Reg[rd_idx] = (rn_lo * rm_lo) - (rn_hi * rm_hi);
                 }
                 // SMLAD
-                else {
-                    const u8 ra_idx = BITS(12, 15);
+                else if ((instr & 0xd0) == 0x10) {
                     state->Reg[rd_idx] = (rn_lo * rm_lo) + (rn_hi * rm_hi) + (s32)state->Reg[ra_idx];
                 }
+                // SMLSD
+                else {
+                    state->Reg[rd_idx] = ((rn_lo * rm_lo) - (rn_hi * rm_hi)) + (s32)state->Reg[ra_idx];
+                }
                 return 1;
-            } else {
-                printf ("Unhandled v6 insn: smlsd\n");
             }
             break;
         case 0x74:
@@ -6357,7 +6453,30 @@ L_stm_s_takeabort:
             printf ("Unhandled v6 insn: smmla/smmls/smmul\n");
             break;
         case 0x78:
-            printf ("Unhandled v6 insn: usad/usada8\n");
+            if (BITS(20, 24) == 0x18)
+            {
+                const u8 rm_idx = BITS(8, 11);
+                const u8 rn_idx = BITS(0, 3);
+                const u8 rd_idx = BITS(16, 19);
+
+                const u32 rm_val = state->Reg[rm_idx];
+                const u32 rn_val = state->Reg[rn_idx];
+
+                const u8 diff1 = (u8)std::labs((rn_val & 0xFF) - (rm_val & 0xFF));
+                const u8 diff2 = (u8)std::labs(((rn_val >> 8) & 0xFF) - ((rm_val >> 8) & 0xFF));
+                const u8 diff3 = (u8)std::labs(((rn_val >> 16) & 0xFF) - ((rm_val >> 16) & 0xFF));
+                const u8 diff4 = (u8)std::labs(((rn_val >> 24) & 0xFF) - ((rm_val >> 24) & 0xFF));
+
+                u32 finalDif = (diff1 + diff2 + diff3 + diff4);
+
+                // Op is USADA8 if true.
+                const u8 ra_idx = BITS(12, 15);
+                if (ra_idx != 15)
+                    finalDif += state->Reg[ra_idx];
+
+                state->Reg[rd_idx] = finalDif;
+                return 1;
+            }
             break;
         case 0x7a:
             printf ("Unhandled v6 insn: usbfx\n");
diff --git a/src/core/file_sys/archive_backend.h b/src/core/file_sys/archive_backend.h
index 1b510b695..43583d206 100644
--- a/src/core/file_sys/archive_backend.h
+++ b/src/core/file_sys/archive_backend.h
@@ -143,7 +143,16 @@ public:
             case Char:
                 return std::vector<u8>(string.begin(), string.end());
             case Wchar:
-                return std::vector<u8>(u16str.begin(), u16str.end());
+            {
+                // use two u8 for each character of u16str
+                std::vector<u8> to_return(u16str.size() * 2);
+                for (size_t i = 0; i < u16str.size(); ++i) {
+                    u16 tmp_char = u16str.at(i);
+                    to_return[i*2] = (tmp_char & 0xFF00) >> 8;
+                    to_return[i*2 + 1] = (tmp_char & 0x00FF);
+                }
+                return to_return;
+            }
             case Empty:
                 return {};
             default:
diff --git a/src/core/hle/kernel/kernel.h b/src/core/hle/kernel/kernel.h
index 7123485be..683fffeee 100644
--- a/src/core/hle/kernel/kernel.h
+++ b/src/core/hle/kernel/kernel.h
@@ -14,6 +14,10 @@ typedef s32 Result;
 
 namespace Kernel {
 
+// From kernel.h. Declarations duplicated here to avoid a circular header dependency.
+class Thread;
+Thread* GetCurrentThread();
+
 enum KernelHandle {
     CurrentThread   = 0xFFFF8000,
     CurrentProcess  = 0xFFFF8001,
@@ -81,6 +85,10 @@ public:
 
     template <class T>
     T* Get(Handle handle) {
+        if (handle == CurrentThread) {
+            return reinterpret_cast<T*>(GetCurrentThread());
+        }
+
         if (handle < HANDLE_OFFSET || handle >= HANDLE_OFFSET + MAX_COUNT || !occupied[handle - HANDLE_OFFSET]) {
             if (handle != 0) {
                 LOG_ERROR(Kernel, "Bad object handle %08x", handle);
@@ -99,6 +107,10 @@ public:
     // ONLY use this when you know the handle is valid.
     template <class T>
     T *GetFast(Handle handle) {
+        if (handle == CurrentThread) {
+            return reinterpret_cast<T*>(GetCurrentThread());
+        }
+
         const Handle realHandle = handle - HANDLE_OFFSET;
         _dbg_assert_(Kernel, realHandle >= 0 && realHandle < MAX_COUNT && occupied[realHandle]);
         return static_cast<T*>(pool[realHandle]);
diff --git a/src/core/hle/kernel/semaphore.cpp b/src/core/hle/kernel/semaphore.cpp
index 6f56da8a9..f955d1957 100644
--- a/src/core/hle/kernel/semaphore.cpp
+++ b/src/core/hle/kernel/semaphore.cpp
@@ -20,8 +20,8 @@ public:
     static Kernel::HandleType GetStaticHandleType() { return Kernel::HandleType::Semaphore; }
     Kernel::HandleType GetHandleType() const override { return Kernel::HandleType::Semaphore; }
 
-    u32 max_count;                              ///< Maximum number of simultaneous holders the semaphore can have
-    u32 available_count;                        ///< Number of free slots left in the semaphore
+    s32 max_count;                              ///< Maximum number of simultaneous holders the semaphore can have
+    s32 available_count;                        ///< Number of free slots left in the semaphore
     std::queue<Handle> waiting_threads;         ///< Threads that are waiting for the semaphore
     std::string name;                           ///< Name of semaphore (optional)
 
@@ -49,8 +49,8 @@ public:
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////
 
-ResultCode CreateSemaphore(Handle* handle, u32 initial_count, 
-    u32 max_count, const std::string& name) {
+ResultCode CreateSemaphore(Handle* handle, s32 initial_count, 
+    s32 max_count, const std::string& name) {
 
     if (initial_count > max_count)
         return ResultCode(ErrorDescription::InvalidCombination, ErrorModule::Kernel,
diff --git a/src/core/hle/kernel/semaphore.h b/src/core/hle/kernel/semaphore.h
index f0075fdb8..ad474b875 100644
--- a/src/core/hle/kernel/semaphore.h
+++ b/src/core/hle/kernel/semaphore.h
@@ -18,7 +18,7 @@ namespace Kernel {
  * @param name Optional name of semaphore
  * @return ResultCode of the error
  */
-ResultCode CreateSemaphore(Handle* handle, u32 initial_count, u32 max_count, const std::string& name = "Unknown");
+ResultCode CreateSemaphore(Handle* handle, s32 initial_count, s32 max_count, const std::string& name = "Unknown");
 
 /**
  * Releases a certain number of slots from a semaphore.
diff --git a/src/core/hle/kernel/thread.cpp b/src/core/hle/kernel/thread.cpp
index 1c04701de..834308926 100644
--- a/src/core/hle/kernel/thread.cpp
+++ b/src/core/hle/kernel/thread.cpp
@@ -83,8 +83,7 @@ static Thread* current_thread;
 static const u32 INITIAL_THREAD_ID = 1; ///< The first available thread id at startup
 static u32 next_thread_id; ///< The next available thread id
 
-/// Gets the current thread
-inline Thread* GetCurrentThread() {
+Thread* GetCurrentThread() {
     return current_thread;
 }
 
@@ -148,16 +147,19 @@ void ChangeReadyState(Thread* t, bool ready) {
     }
 }
 
-/// Verify that a thread has not been released from waiting
-static bool VerifyWait(const Thread* thread, WaitType type, Handle wait_handle) {
-    _dbg_assert_(Kernel, thread != nullptr);
-    return (type == thread->wait_type) && (wait_handle == thread->wait_handle) && (thread->IsWaiting());
+/// Check if a thread is blocking on a specified wait type
+static bool CheckWaitType(const Thread* thread, WaitType type) {
+    return (type == thread->wait_type) && (thread->IsWaiting());
 }
 
-/// Verify that a thread has not been released from waiting (with wait address)
-static bool VerifyWait(const Thread* thread, WaitType type, Handle wait_handle, VAddr wait_address) {
-    _dbg_assert_(Kernel, thread != nullptr);
-    return VerifyWait(thread, type, wait_handle) && (wait_address == thread->wait_address);
+/// Check if a thread is blocking on a specified wait type with a specified handle
+static bool CheckWaitType(const Thread* thread, WaitType type, Handle wait_handle) {
+    return CheckWaitType(thread, type) && (wait_handle == thread->wait_handle);
+}
+
+/// Check if a thread is blocking on a specified wait type with a specified handle and address
+static bool CheckWaitType(const Thread* thread, WaitType type, Handle wait_handle, VAddr wait_address) {
+    return CheckWaitType(thread, type, wait_handle) && (wait_address == thread->wait_address);
 }
 
 /// Stops the current thread
@@ -172,9 +174,9 @@ ResultCode StopThread(Handle handle, const char* reason) {
     thread->status = THREADSTATUS_DORMANT;
     for (Handle waiting_handle : thread->waiting_threads) {
         Thread* waiting_thread = g_object_pool.Get<Thread>(waiting_handle);
-        if (VerifyWait(waiting_thread, WAITTYPE_THREADEND, handle)) {
+
+        if (CheckWaitType(waiting_thread, WAITTYPE_THREADEND, handle))
             ResumeThreadFromWait(waiting_handle);
-        }
     }
     thread->waiting_threads.clear();
 
@@ -210,7 +212,7 @@ Handle ArbitrateHighestPriorityThread(u32 arbiter, u32 address) {
     for (Handle handle : thread_queue) {
         Thread* thread = g_object_pool.Get<Thread>(handle);
 
-        if (!VerifyWait(thread, WAITTYPE_ARB, arbiter, address))
+        if (!CheckWaitType(thread, WAITTYPE_ARB, arbiter, address))
             continue;
 
         if (thread == nullptr)
@@ -235,7 +237,7 @@ void ArbitrateAllThreads(u32 arbiter, u32 address) {
     for (Handle handle : thread_queue) {
         Thread* thread = g_object_pool.Get<Thread>(handle);
 
-        if (VerifyWait(thread, WAITTYPE_ARB, arbiter, address))
+        if (CheckWaitType(thread, WAITTYPE_ARB, arbiter, address))
             ResumeThreadFromWait(handle);
     }
 }
@@ -306,6 +308,8 @@ void ResumeThreadFromWait(Handle handle) {
     Thread* thread = Kernel::g_object_pool.Get<Thread>(handle);
     if (thread) {
         thread->status &= ~THREADSTATUS_WAIT;
+        thread->wait_handle = 0;
+        thread->wait_type = WAITTYPE_NONE;
         if (!(thread->status & (THREADSTATUS_WAITSUSPEND | THREADSTATUS_DORMANT | THREADSTATUS_DEAD))) {
             ChangeReadyState(thread, true);
         }
@@ -469,19 +473,27 @@ void Reschedule() {
     Thread* prev = GetCurrentThread();
     Thread* next = NextThread();
     HLE::g_reschedule = false;
-    if (next > 0) {
-        LOG_TRACE(Kernel, "context switch 0x%08X -> 0x%08X", prev->GetHandle(), next->GetHandle());
 
+    if (next != nullptr) {
+        LOG_TRACE(Kernel, "context switch 0x%08X -> 0x%08X", prev->GetHandle(), next->GetHandle());
         SwitchContext(next);
+    } else {
+        LOG_TRACE(Kernel, "cannot context switch from 0x%08X, no higher priority thread!", prev->GetHandle());
 
-        // Hack - There is no mechanism yet to waken the primary thread if it has been put to sleep
-        // by a simulated VBLANK thread switch. So, we'll just immediately set it to "ready" again.
-        // This results in the current thread yielding on a VBLANK once, and then it will be
-        // immediately placed back in the queue for execution.
-        if (prev->wait_type == WAITTYPE_VBLANK) {
-            ResumeThreadFromWait(prev->GetHandle());
+        for (Handle handle : thread_queue) {
+            Thread* thread = g_object_pool.Get<Thread>(handle);
+            LOG_TRACE(Kernel, "\thandle=0x%08X prio=0x%02X, status=0x%08X wait_type=0x%08X wait_handle=0x%08X",
+                thread->GetHandle(), thread->current_priority, thread->status, thread->wait_type, thread->wait_handle);
         }
     }
+
+    // TODO(bunnei): Hack - There is no timing mechanism yet to wake up a thread if it has been put
+    // to sleep. So, we'll just immediately set it to "ready" again after an attempted context
+    // switch has occurred. This results in the current thread yielding on a sleep once, and then it
+    // will immediately be placed back in the queue for execution.
+
+    if (CheckWaitType(prev, WAITTYPE_SLEEP))
+        ResumeThreadFromWait(prev->GetHandle());
 }
 
 ResultCode GetThreadId(u32* thread_id, Handle handle) {
diff --git a/src/core/hle/kernel/thread.h b/src/core/hle/kernel/thread.h
index be7adface..65e8ef554 100644
--- a/src/core/hle/kernel/thread.h
+++ b/src/core/hle/kernel/thread.h
@@ -40,7 +40,6 @@ enum WaitType {
     WAITTYPE_SEMA,
     WAITTYPE_EVENT,
     WAITTYPE_THREADEND,
-    WAITTYPE_VBLANK,
     WAITTYPE_MUTEX,
     WAITTYPE_SYNCH,
     WAITTYPE_ARB,
@@ -78,6 +77,9 @@ Handle ArbitrateHighestPriorityThread(u32 arbiter, u32 address);
 /// Arbitrate all threads currently waiting...
 void ArbitrateAllThreads(u32 arbiter, u32 address);
 
+/// Gets the current thread
+Thread* GetCurrentThread();
+
 /// Gets the current thread handle
 Handle GetCurrentThreadHandle();
 
diff --git a/src/core/hle/svc.cpp b/src/core/hle/svc.cpp
index 47e9bf77e..70ef7839c 100644
--- a/src/core/hle/svc.cpp
+++ b/src/core/hle/svc.cpp
@@ -352,7 +352,8 @@ static Result ClearEvent(Handle evt) {
 static void SleepThread(s64 nanoseconds) {
     LOG_TRACE(Kernel_SVC, "called nanoseconds=%lld", nanoseconds);
 
-    // Check for next thread to schedule
+    // Sleep current thread and check for next thread to schedule
+    Kernel::WaitCurrentThread(WAITTYPE_SLEEP);
     HLE::Reschedule(__func__);
 }
 
diff --git a/src/core/loader/3dsx.cpp b/src/core/loader/3dsx.cpp
index 0437e5374..3d84fc5da 100644
--- a/src/core/loader/3dsx.cpp
+++ b/src/core/loader/3dsx.cpp
@@ -223,9 +223,7 @@ int THREEDSXReader::Load3DSXFile(const std::string& filename, u32 base_addr)
         LOG_INFO(Loader, "Loading 3DSX file %s...", filename.c_str());
         FileUtil::IOFile file(filename, "rb");
         if (file.IsOpen()) {
-
-            THREEDSXReader reader;
-            reader.Load3DSXFile(filename, 0x00100000);
+            THREEDSXReader::Load3DSXFile(filename, 0x00100000);
             Kernel::LoadExec(0x00100000);
         } else {
             return ResultStatus::Error;