258 files changed, 12670 insertions, 12088 deletions

diff --git a/src/citra/CMakeLists.txt b/src/citra/CMakeLists.txt
index f2add394f..b06259f5e 100644
--- a/src/citra/CMakeLists.txt
+++ b/src/citra/CMakeLists.txt
@@ -12,22 +12,20 @@ set(HEADERS
 
 create_directory_groups(${SRCS} ${HEADERS})
 
-# NOTE: This is a workaround for CMake bug 0006976 (missing X11_xf86vmode_LIB variable)
-if (NOT X11_xf86vmode_LIB)
-    set(X11_xv86vmode_LIB Xxf86vm)
-endif()
-
 add_executable(citra ${SRCS} ${HEADERS})
 target_link_libraries(citra core common video_core)
 target_link_libraries(citra ${OPENGL_gl_LIBRARY} ${GLFW_LIBRARIES} inih)
 
+if (UNIX)
+    target_link_libraries(citra -pthread)
+endif()
+
 if (APPLE)
-    target_link_libraries(citra iconv pthread ${COREFOUNDATION_LIBRARY})
+    target_link_libraries(citra iconv ${COREFOUNDATION_LIBRARY})
 elseif (WIN32)
     target_link_libraries(citra winmm)
 else() # Unix
-    target_link_libraries(citra pthread rt)
-    target_link_libraries(citra ${X11_X11_LIB} ${X11_Xi_LIB} ${X11_Xcursor_LIB} ${X11_Xrandr_LIB} ${X11_xv86vmode_LIB})
+    target_link_libraries(citra rt)
 endif()
 
 #install(TARGETS citra RUNTIME DESTINATION ${bindir})
diff --git a/src/citra/citra.cpp b/src/citra/citra.cpp
index d6e8a4ec7..f6a52758b 100644
--- a/src/citra/citra.cpp
+++ b/src/citra/citra.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include <thread>
diff --git a/src/citra/config.cpp b/src/citra/config.cpp
index 92764809e..2bf0dff35 100644
--- a/src/citra/config.cpp
+++ b/src/citra/config.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include <GLFW/glfw3.h>
@@ -58,7 +58,8 @@ void Config::ReadValues() {
 
     // Core
     Settings::values.cpu_core = glfw_config->GetInteger("Core", "cpu_core", Core::CPU_Interpreter);
-    Settings::values.gpu_refresh_rate = glfw_config->GetInteger("Core", "gpu_refresh_rate", 60);
+    Settings::values.gpu_refresh_rate = glfw_config->GetInteger("Core", "gpu_refresh_rate", 30);
+    Settings::values.frame_skip = glfw_config->GetInteger("Core", "frame_skip", 0);
 
     // Data Storage
     Settings::values.use_virtual_sd = glfw_config->GetBoolean("Data Storage", "use_virtual_sd", true);
diff --git a/src/citra/config.h b/src/citra/config.h
index 2b46fa8aa..0eb176c7d 100644
--- a/src/citra/config.h
+++ b/src/citra/config.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/citra/default_ini.h b/src/citra/default_ini.h
index 7cf543e07..f41020f7b 100644
--- a/src/citra/default_ini.h
+++ b/src/citra/default_ini.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -28,7 +28,8 @@ pad_sright =
 
 [Core]
 cpu_core = ## 0: Interpreter (default), 1: FastInterpreter (experimental)
-gpu_refresh_rate = ## 60 (default)
+gpu_refresh_rate = ## 30 (default)
+frame_skip = ## 0: No frameskip (default), 1 : 2x frameskip, 2 : 4x frameskip, etc.
 
 [Data Storage]
 use_virtual_sd =
diff --git a/src/citra/emu_window/emu_window_glfw.cpp b/src/citra/emu_window/emu_window_glfw.cpp
index 929e09f43..a6282809b 100644
--- a/src/citra/emu_window/emu_window_glfw.cpp
+++ b/src/citra/emu_window/emu_window_glfw.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include <GLFW/glfw3.h>
diff --git a/src/citra/emu_window/emu_window_glfw.h b/src/citra/emu_window/emu_window_glfw.h
index 5b04e87bb..5252fccc8 100644
--- a/src/citra/emu_window/emu_window_glfw.h
+++ b/src/citra/emu_window/emu_window_glfw.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/citra_qt/CMakeLists.txt b/src/citra_qt/CMakeLists.txt
index 90e5c6aa6..54d0a1271 100644
--- a/src/citra_qt/CMakeLists.txt
+++ b/src/citra_qt/CMakeLists.txt
@@ -60,6 +60,10 @@ add_executable(citra-qt ${SRCS} ${HEADERS} ${UI_HDRS})
 target_link_libraries(citra-qt core common video_core qhexedit)
 target_link_libraries(citra-qt ${OPENGL_gl_LIBRARY} ${CITRA_QT_LIBS})
 
+if (UNIX)
+    target_link_libraries(citra-qt -pthread)
+endif()
+
 if (APPLE)
     target_link_libraries(citra-qt iconv ${COREFOUNDATION_LIBRARY})
 elseif (WIN32)
diff --git a/src/citra_qt/config.cpp b/src/citra_qt/config.cpp
index 0ae6b8b2d..1596c08d7 100644
--- a/src/citra_qt/config.cpp
+++ b/src/citra_qt/config.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include <QString>
@@ -44,7 +44,8 @@ void Config::ReadValues() {
 
     qt_config->beginGroup("Core");
     Settings::values.cpu_core = qt_config->value("cpu_core", Core::CPU_Interpreter).toInt();
-    Settings::values.gpu_refresh_rate = qt_config->value("gpu_refresh_rate", 60).toInt();
+    Settings::values.gpu_refresh_rate = qt_config->value("gpu_refresh_rate", 30).toInt();
+    Settings::values.frame_skip = qt_config->value("frame_skip", 0).toInt();
     qt_config->endGroup();
 
     qt_config->beginGroup("Data Storage");
@@ -80,6 +81,7 @@ void Config::SaveValues() {
     qt_config->beginGroup("Core");
     qt_config->setValue("cpu_core", Settings::values.cpu_core);
     qt_config->setValue("gpu_refresh_rate", Settings::values.gpu_refresh_rate);
+    qt_config->setValue("frame_skip", Settings::values.frame_skip);
     qt_config->endGroup();
 
     qt_config->beginGroup("Data Storage");
diff --git a/src/citra_qt/config.h b/src/citra_qt/config.h
index 4c95d0cb9..4485cae73 100644
--- a/src/citra_qt/config.h
+++ b/src/citra_qt/config.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/citra_qt/debugger/callstack.cpp b/src/citra_qt/debugger/callstack.cpp
index 895851be3..a9ec2f7fe 100644
--- a/src/citra_qt/debugger/callstack.cpp
+++ b/src/citra_qt/debugger/callstack.cpp
@@ -27,10 +27,10 @@ void CallstackWidget::OnCPUStepped()
     ARM_Interface* app_core = Core::g_app_core;
 
     u32 sp = app_core->GetReg(13); //stack pointer
-    u32 addr, ret_addr, call_addr, func_addr;
+    u32 ret_addr, call_addr, func_addr;
 
     int counter = 0;
-    for (int addr = 0x10000000; addr >= sp; addr -= 4)
+    for (u32 addr = 0x10000000; addr >= sp; addr -= 4)
     {
         ret_addr = Memory::Read32(addr);
         call_addr = ret_addr - 4; //get call address???
diff --git a/src/citra_qt/debugger/disassembler.cpp b/src/citra_qt/debugger/disassembler.cpp
index 2ee877743..14745f3bb 100644
--- a/src/citra_qt/debugger/disassembler.cpp
+++ b/src/citra_qt/debugger/disassembler.cpp
@@ -220,7 +220,9 @@ void DisassemblerWidget::OnPause()
     emu_thread.SetCpuRunning(false);
 
     // TODO: By now, the CPU might not have actually stopped...
-    model->SetNextInstruction(Core::g_app_core->GetPC());
+    if (Core::g_app_core) {
+        model->SetNextInstruction(Core::g_app_core->GetPC());
+    }
 }
 
 void DisassemblerWidget::OnToggleStartStop()
diff --git a/src/citra_qt/debugger/graphics.cpp b/src/citra_qt/debugger/graphics.cpp
index a86a55404..6ff4c290d 100644
--- a/src/citra_qt/debugger/graphics.cpp
+++ b/src/citra_qt/debugger/graphics.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "graphics.hxx"
diff --git a/src/citra_qt/debugger/graphics.hxx b/src/citra_qt/debugger/graphics.hxx
index 72656f93c..8119b4c87 100644
--- a/src/citra_qt/debugger/graphics.hxx
+++ b/src/citra_qt/debugger/graphics.hxx
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/citra_qt/debugger/graphics_breakpoints.cpp b/src/citra_qt/debugger/graphics_breakpoints.cpp
index 53394b6e6..9486f06cc 100644
--- a/src/citra_qt/debugger/graphics_breakpoints.cpp
+++ b/src/citra_qt/debugger/graphics_breakpoints.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include <QMetaType>
@@ -39,15 +39,17 @@ QVariant BreakPointModel::data(const QModelIndex& index, int role) const
         switch (index.column()) {
         case 0:
         {
-            std::map<Pica::DebugContext::Event, QString> map;
-            map.insert({Pica::DebugContext::Event::CommandLoaded, tr("Pica command loaded")});
-            map.insert({Pica::DebugContext::Event::CommandProcessed, tr("Pica command processed")});
-            map.insert({Pica::DebugContext::Event::IncomingPrimitiveBatch, tr("Incoming primitive batch")});
-            map.insert({Pica::DebugContext::Event::FinishedPrimitiveBatch, tr("Finished primitive batch")});
+            static const std::map<Pica::DebugContext::Event, QString> map = {
+                { Pica::DebugContext::Event::CommandLoaded, tr("Pica command loaded") },
+                { Pica::DebugContext::Event::CommandProcessed, tr("Pica command processed") },
+                { Pica::DebugContext::Event::IncomingPrimitiveBatch, tr("Incoming primitive batch") },
+                { Pica::DebugContext::Event::FinishedPrimitiveBatch, tr("Finished primitive batch") },
+                { Pica::DebugContext::Event::VertexLoaded, tr("Vertex Loaded") }
+            };
 
             _dbg_assert_(Debug_GPU, map.size() == static_cast<size_t>(Pica::DebugContext::Event::NumEvents));
 
-            return map[event];
+            return (map.find(event) != map.end()) ? map.at(event) : QString();
         }
 
         case 1:
diff --git a/src/citra_qt/debugger/graphics_breakpoints.hxx b/src/citra_qt/debugger/graphics_breakpoints.hxx
index 2142c6fa1..5b9ba324e 100644
--- a/src/citra_qt/debugger/graphics_breakpoints.hxx
+++ b/src/citra_qt/debugger/graphics_breakpoints.hxx
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/citra_qt/debugger/graphics_breakpoints_p.hxx b/src/citra_qt/debugger/graphics_breakpoints_p.hxx
index bf5daf73d..232bfc863 100644
--- a/src/citra_qt/debugger/graphics_breakpoints_p.hxx
+++ b/src/citra_qt/debugger/graphics_breakpoints_p.hxx
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/citra_qt/debugger/graphics_cmdlists.cpp b/src/citra_qt/debugger/graphics_cmdlists.cpp
index 7f97cf143..753cc25da 100644
--- a/src/citra_qt/debugger/graphics_cmdlists.cpp
+++ b/src/citra_qt/debugger/graphics_cmdlists.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include <QLabel>
@@ -24,7 +24,7 @@ QImage LoadTexture(u8* src, const Pica::DebugUtils::TextureInfo& info) {
     QImage decoded_image(info.width, info.height, QImage::Format_ARGB32);
     for (int y = 0; y < info.height; ++y) {
         for (int x = 0; x < info.width; ++x) {
-            Math::Vec4<u8> color = Pica::DebugUtils::LookupTexture(src, x, y, info);
+            Math::Vec4<u8> color = Pica::DebugUtils::LookupTexture(src, x, y, info, true);
             decoded_image.setPixel(x, y, qRgba(color.r(), color.g(), color.b(), color.a()));
         }
     }
@@ -47,7 +47,7 @@ public:
 };
 
 TextureInfoDockWidget::TextureInfoDockWidget(const Pica::DebugUtils::TextureInfo& info, QWidget* parent)
-    : QDockWidget(tr("Texture 0x%1").arg(info.address, 8, 16, QLatin1Char('0'))),
+    : QDockWidget(tr("Texture 0x%1").arg(info.physical_address, 8, 16, QLatin1Char('0'))),
       info(info) {
 
     QWidget* main_widget = new QWidget;
@@ -60,7 +60,7 @@ TextureInfoDockWidget::TextureInfoDockWidget(const Pica::DebugUtils::TextureInfo
     phys_address_spinbox->SetBase(16);
     phys_address_spinbox->SetRange(0, 0xFFFFFFFF);
     phys_address_spinbox->SetPrefix("0x");
-    phys_address_spinbox->SetValue(info.address);
+    phys_address_spinbox->SetValue(info.physical_address);
     connect(phys_address_spinbox, SIGNAL(ValueChanged(qint64)), this, SLOT(OnAddressChanged(qint64)));
 
     QComboBox* format_choice = new QComboBox;
@@ -69,6 +69,13 @@ TextureInfoDockWidget::TextureInfoDockWidget(const Pica::DebugUtils::TextureInfo
     format_choice->addItem(tr("RGBA5551"));
     format_choice->addItem(tr("RGB565"));
     format_choice->addItem(tr("RGBA4"));
+    format_choice->addItem(tr("IA8"));
+    format_choice->addItem(tr("UNK6"));
+    format_choice->addItem(tr("I8"));
+    format_choice->addItem(tr("A8"));
+    format_choice->addItem(tr("IA4"));
+    format_choice->addItem(tr("UNK10"));
+    format_choice->addItem(tr("A4"));
     format_choice->setCurrentIndex(static_cast<int>(info.format));
     connect(format_choice, SIGNAL(currentIndexChanged(int)), this, SLOT(OnFormatChanged(int)));
 
@@ -125,7 +132,7 @@ TextureInfoDockWidget::TextureInfoDockWidget(const Pica::DebugUtils::TextureInfo
 }
 
 void TextureInfoDockWidget::OnAddressChanged(qint64 value) {
-    info.address = value;
+    info.physical_address = value;
     emit UpdatePixmap(ReloadPixmap());
 }
 
@@ -150,7 +157,7 @@ void TextureInfoDockWidget::OnStrideChanged(int value) {
 }
 
 QPixmap TextureInfoDockWidget::ReloadPixmap() const {
-    u8* src = Memory::GetPointer(info.address);
+    u8* src = Memory::GetPointer(Pica::PAddrToVAddr(info.physical_address));
     return QPixmap::fromImage(LoadTexture(src, info));
 }
 
@@ -223,9 +230,21 @@ void GPUCommandListModel::OnPicaTraceFinished(const Pica::DebugUtils::PicaTrace&
 
 void GPUCommandListWidget::OnCommandDoubleClicked(const QModelIndex& index) {
     const int command_id = list_widget->model()->data(index, GPUCommandListModel::CommandIdRole).toInt();
-    if (COMMAND_IN_RANGE(command_id, texture0)) {
-        auto info = Pica::DebugUtils::TextureInfo::FromPicaRegister(Pica::registers.texture0,
-                                                                    Pica::registers.texture0_format);
+    if (COMMAND_IN_RANGE(command_id, texture0) ||
+        COMMAND_IN_RANGE(command_id, texture1) ||
+        COMMAND_IN_RANGE(command_id, texture2)) {
+
+        unsigned index;
+        if (COMMAND_IN_RANGE(command_id, texture0)) {
+            index = 0;
+        } else if (COMMAND_IN_RANGE(command_id, texture1)) {
+            index = 1;
+        } else {
+            index = 2;
+        }
+        auto config = Pica::registers.GetTextures()[index].config;
+        auto format = Pica::registers.GetTextures()[index].format;
+        auto info = Pica::DebugUtils::TextureInfo::FromPicaRegister(config, format);
 
         // TODO: Instead, emit a signal here to be caught by the main window widget.
         auto main_window = static_cast<QMainWindow*>(parent());
@@ -237,10 +256,23 @@ void GPUCommandListWidget::SetCommandInfo(const QModelIndex& index) {
     QWidget* new_info_widget;
 
     const int command_id = list_widget->model()->data(index, GPUCommandListModel::CommandIdRole).toInt();
-    if (COMMAND_IN_RANGE(command_id, texture0)) {
-        u8* src = Memory::GetPointer(Pica::registers.texture0.GetPhysicalAddress());
-        auto info = Pica::DebugUtils::TextureInfo::FromPicaRegister(Pica::registers.texture0,
-                                                                    Pica::registers.texture0_format);
+    if (COMMAND_IN_RANGE(command_id, texture0) ||
+        COMMAND_IN_RANGE(command_id, texture1) ||
+        COMMAND_IN_RANGE(command_id, texture2)) {
+
+        unsigned index;
+        if (COMMAND_IN_RANGE(command_id, texture0)) {
+            index = 0;
+        } else if (COMMAND_IN_RANGE(command_id, texture1)) {
+            index = 1;
+        } else {
+            index = 2;
+        }
+        auto config = Pica::registers.GetTextures()[index].config;
+        auto format = Pica::registers.GetTextures()[index].format;
+
+        auto info = Pica::DebugUtils::TextureInfo::FromPicaRegister(config, format);
+        u8* src = Memory::GetPointer(Pica::PAddrToVAddr(config.GetPhysicalAddress()));
         new_info_widget = new TextureInfoWidget(src, info);
     } else {
         new_info_widget = new QWidget;
diff --git a/src/citra_qt/debugger/graphics_cmdlists.hxx b/src/citra_qt/debugger/graphics_cmdlists.hxx
index a459bba64..a465d044c 100644
--- a/src/citra_qt/debugger/graphics_cmdlists.hxx
+++ b/src/citra_qt/debugger/graphics_cmdlists.hxx
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/citra_qt/debugger/graphics_framebuffer.cpp b/src/citra_qt/debugger/graphics_framebuffer.cpp
index ac47f298d..dd41c3880 100644
--- a/src/citra_qt/debugger/graphics_framebuffer.cpp
+++ b/src/citra_qt/debugger/graphics_framebuffer.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include <QBoxLayout>
@@ -125,7 +125,8 @@ GraphicsFramebufferWidget::GraphicsFramebufferWidget(std::shared_ptr<Pica::Debug
     setWidget(main_widget);
 
     // Load current data - TODO: Make sure this works when emulation is not running
-    emit Update();
+    if (debug_context && debug_context->at_breakpoint)
+        emit Update();
     widget()->setEnabled(false); // TODO: Only enable if currently at breakpoint
 }
 
@@ -198,7 +199,7 @@ void GraphicsFramebufferWidget::OnUpdate()
         auto framebuffer = Pica::registers.framebuffer;
         using Framebuffer = decltype(framebuffer);
 
-        framebuffer_address = framebuffer.GetColorBufferAddress();
+        framebuffer_address = framebuffer.GetColorBufferPhysicalAddress();
         framebuffer_width = framebuffer.GetWidth();
         framebuffer_height = framebuffer.GetHeight();
         framebuffer_format = static_cast<Format>(framebuffer.color_format);
@@ -223,9 +224,9 @@ void GraphicsFramebufferWidget::OnUpdate()
     case Format::RGBA8:
     {
         QImage decoded_image(framebuffer_width, framebuffer_height, QImage::Format_ARGB32);
-        u32* color_buffer = (u32*)Memory::GetPointer(framebuffer_address);
-        for (int y = 0; y < framebuffer_height; ++y) {
-            for (int x = 0; x < framebuffer_width; ++x) {
+        u32* color_buffer = (u32*)Memory::GetPointer(Pica::PAddrToVAddr(framebuffer_address));
+        for (unsigned y = 0; y < framebuffer_height; ++y) {
+            for (unsigned x = 0; x < framebuffer_width; ++x) {
                 u32 value = *(color_buffer + x + y * framebuffer_width);
 
                 decoded_image.setPixel(x, y, qRgba((value >> 16) & 0xFF, (value >> 8) & 0xFF, value & 0xFF, 255/*value >> 24*/));
@@ -238,9 +239,9 @@ void GraphicsFramebufferWidget::OnUpdate()
     case Format::RGB8:
     {
         QImage decoded_image(framebuffer_width, framebuffer_height, QImage::Format_ARGB32);
-        u8* color_buffer = Memory::GetPointer(framebuffer_address);
-        for (int y = 0; y < framebuffer_height; ++y) {
-            for (int x = 0; x < framebuffer_width; ++x) {
+        u8* color_buffer = Memory::GetPointer(Pica::PAddrToVAddr(framebuffer_address));
+        for (unsigned y = 0; y < framebuffer_height; ++y) {
+            for (unsigned x = 0; x < framebuffer_width; ++x) {
                 u8* pixel_pointer = color_buffer + x * 3 + y * 3 * framebuffer_width;
 
                 decoded_image.setPixel(x, y, qRgba(pixel_pointer[0], pixel_pointer[1], pixel_pointer[2], 255/*value >> 24*/));
@@ -253,9 +254,9 @@ void GraphicsFramebufferWidget::OnUpdate()
     case Format::RGBA5551:
     {
         QImage decoded_image(framebuffer_width, framebuffer_height, QImage::Format_ARGB32);
-        u32* color_buffer = (u32*)Memory::GetPointer(framebuffer_address);
-        for (int y = 0; y < framebuffer_height; ++y) {
-            for (int x = 0; x < framebuffer_width; ++x) {
+        u32* color_buffer = (u32*)Memory::GetPointer(Pica::PAddrToVAddr(framebuffer_address));
+        for (unsigned y = 0; y < framebuffer_height; ++y) {
+            for (unsigned x = 0; x < framebuffer_width; ++x) {
                 u16 value = *(u16*)(((u8*)color_buffer) + x * 2 + y * framebuffer_width * 2);
                 u8 r = (value >> 11) & 0x1F;
                 u8 g = (value >> 6) & 0x1F;
diff --git a/src/citra_qt/debugger/graphics_framebuffer.hxx b/src/citra_qt/debugger/graphics_framebuffer.hxx
index 1151ee7a1..56215761e 100644
--- a/src/citra_qt/debugger/graphics_framebuffer.hxx
+++ b/src/citra_qt/debugger/graphics_framebuffer.hxx
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/citra_qt/main.cpp b/src/citra_qt/main.cpp
index 1299338ac..b12e6a02b 100644
--- a/src/citra_qt/main.cpp
+++ b/src/citra_qt/main.cpp
@@ -157,12 +157,6 @@ void GMainWindow::BootGame(std::string filename)
     LOG_INFO(Frontend, "Citra starting...\n");
     System::Init(render_window);
 
-    if (Core::Init()) {
-        LOG_CRITICAL(Frontend, "Core initialization failed, exiting...");
-        Core::Stop();
-        exit(1);
-    }
-
     // Load a game or die...
     if (Loader::ResultStatus::Success != Loader::LoadFile(filename)) {
         LOG_CRITICAL(Frontend, "Failed to load ROM!");
@@ -236,12 +230,15 @@ void GMainWindow::ToggleWindowMode()
         render_window->setParent(nullptr);
         render_window->setVisible(true);
         render_window->RestoreGeometry();
+        render_window->setFocusPolicy(Qt::NoFocus);
     }
     else if (!enable && render_window->parent() == nullptr)
     {
         render_window->BackupGeometry();
         ui.horizontalLayout->addWidget(render_window);
         render_window->setVisible(true);
+        render_window->setFocusPolicy(Qt::ClickFocus);
+        render_window->setFocus();
     }
 }
 
diff --git a/src/citra_qt/util/spinbox.cpp b/src/citra_qt/util/spinbox.cpp
index 9672168f5..f9988409f 100644
--- a/src/citra_qt/util/spinbox.cpp
+++ b/src/citra_qt/util/spinbox.cpp
@@ -1,4 +1,4 @@
-// Licensed under GPLv2+
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 
@@ -238,7 +238,7 @@ QValidator::State CSpinBox::validate(QString& input, int& pos) const
     if (!prefix.isEmpty() && input.left(prefix.length()) != prefix)
         return QValidator::Invalid;
 
-    unsigned strpos = prefix.length();
+    int strpos = prefix.length();
 
     // Empty "numbers" allowed as intermediate values
     if (strpos >= input.length() - HasSign() - suffix.length())
diff --git a/src/citra_qt/util/spinbox.hxx b/src/citra_qt/util/spinbox.hxx
index 68f5b9894..ee7f08ec2 100644
--- a/src/citra_qt/util/spinbox.hxx
+++ b/src/citra_qt/util/spinbox.hxx
@@ -1,4 +1,4 @@
-// Licensed under GPLv2+
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 
diff --git a/src/common/CMakeLists.txt b/src/common/CMakeLists.txt
index 15989708d..3c3419bbc 100644
--- a/src/common/CMakeLists.txt
+++ b/src/common/CMakeLists.txt
@@ -49,6 +49,7 @@ set(HEADERS
             logging/filter.h
             logging/log.h
             logging/backend.h
+            make_unique.h
             math_util.h
             mem_arena.h
             memory_util.h
diff --git a/src/common/bit_field.h b/src/common/bit_field.h
index 9e02210f9..8eab054b8 100644
--- a/src/common/bit_field.h
+++ b/src/common/bit_field.h
@@ -1,4 +1,4 @@
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 
@@ -142,7 +142,7 @@ public:
 
     __forceinline BitField& operator=(T val)
     {
-        storage = (storage & ~GetMask()) | (((StorageType)val << position) & GetMask());
+        Assign(val);
         return *this;
     }
 
@@ -151,6 +151,10 @@ public:
         return Value();
     }
 
+    __forceinline void Assign(const T& value) {
+        storage = (storage & ~GetMask()) | (((StorageType)value << position) & GetMask());
+    }
+
     __forceinline T Value() const
     {
         if (std::numeric_limits<T>::is_signed)
@@ -164,6 +168,12 @@ public:
         }
     }
 
+    // TODO: we may want to change this to explicit operator bool() if it's bug-free in VS2015
+    __forceinline bool ToBool() const
+    {
+        return Value() != 0;
+    }
+
 private:
     // StorageType is T for non-enum types and the underlying type of T if
     // T is an enumeration. Note that T is wrapped within an enable_if in the
diff --git a/src/common/break_points.cpp b/src/common/break_points.cpp
index 587dbf40a..6696935fa 100644
--- a/src/common/break_points.cpp
+++ b/src/common/break_points.cpp
@@ -1,5 +1,5 @@
-// Copyright 2013 Dolphin Emulator Project
-// Licensed under GPLv2
+// Copyright 2013 Dolphin Emulator Project / 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "common/common.h"
diff --git a/src/common/break_points.h b/src/common/break_points.h
index cf3884fbc..5557cd50e 100644
--- a/src/common/break_points.h
+++ b/src/common/break_points.h
@@ -1,5 +1,5 @@
-// Copyright 2013 Dolphin Emulator Project
-// Licensed under GPLv2
+// Copyright 2013 Dolphin Emulator Project / 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/common/common.h b/src/common/common.h
index 9f3016d34..66f0ccd0c 100644
--- a/src/common/common.h
+++ b/src/common/common.h
@@ -1,5 +1,5 @@
-// Copyright 2013 Dolphin Emulator Project
-// Licensed under GPLv2
+// Copyright 2013 Dolphin Emulator Project / 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/common/common_funcs.h b/src/common/common_funcs.h
index 67b3679b0..ca7abbea6 100644
--- a/src/common/common_funcs.h
+++ b/src/common/common_funcs.h
@@ -1,5 +1,5 @@
-// Copyright 2013 Dolphin Emulator Project
-// Licensed under GPLv2
+// Copyright 2013 Dolphin Emulator Project / 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/common/common_paths.h b/src/common/common_paths.h
index 42e1a29c1..9d62a8368 100644
--- a/src/common/common_paths.h
+++ b/src/common/common_paths.h
@@ -1,5 +1,5 @@
-// Copyright 2013 Dolphin Emulator Project
-// Licensed under GPLv2
+// Copyright 2013 Dolphin Emulator Project / 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -40,7 +40,9 @@
 #define MAPS_DIR          "maps"
 #define CACHE_DIR         "cache"
 #define SDMC_DIR          "sdmc"
+#define SAVEDATA_DIR      "savedata"
 #define SYSDATA_DIR       "sysdata"
+#define SYSSAVEDATA_DIR   "syssavedata"
 #define SHADERCACHE_DIR   "shader_cache"
 #define STATESAVES_DIR    "state_saves"
 #define SCREENSHOTS_DIR   "screenShots"
diff --git a/src/common/concurrent_ring_buffer.h b/src/common/concurrent_ring_buffer.h
index 2951d93db..311bb01f4 100644
--- a/src/common/concurrent_ring_buffer.h
+++ b/src/common/concurrent_ring_buffer.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2+
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/common/cpu_detect.h b/src/common/cpu_detect.h
index def6afdee..b585f9608 100644
--- a/src/common/cpu_detect.h
+++ b/src/common/cpu_detect.h
@@ -1,5 +1,5 @@
-// Copyright 2013 Dolphin Emulator Project
-// Licensed under GPLv2
+// Copyright 2013 Dolphin Emulator Project / 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 
diff --git a/src/common/emu_window.cpp b/src/common/emu_window.cpp
index 7a2c50ac8..4ec7b263a 100644
--- a/src/common/emu_window.cpp
+++ b/src/common/emu_window.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "emu_window.h"
diff --git a/src/common/emu_window.h b/src/common/emu_window.h
index 4cb94fed1..1ad4b82a3 100644
--- a/src/common/emu_window.h
+++ b/src/common/emu_window.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/common/file_search.cpp b/src/common/file_search.cpp
index bfb54ce72..b3a0a84fb 100644
--- a/src/common/file_search.cpp
+++ b/src/common/file_search.cpp
@@ -1,5 +1,5 @@
-// Copyright 2013 Dolphin Emulator Project
-// Licensed under GPLv2
+// Copyright 2013 Dolphin Emulator Project / 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 
diff --git a/src/common/file_search.h b/src/common/file_search.h
index f966a008d..55ca02638 100644
--- a/src/common/file_search.h
+++ b/src/common/file_search.h
@@ -1,5 +1,5 @@
-// Copyright 2013 Dolphin Emulator Project
-// Licensed under GPLv2
+// Copyright 2013 Dolphin Emulator Project / 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/common/file_util.cpp b/src/common/file_util.cpp
index 88c46c117..bba830c70 100644
--- a/src/common/file_util.cpp
+++ b/src/common/file_util.cpp
@@ -1,5 +1,5 @@
-// Copyright 2013 Dolphin Emulator Project
-// Licensed under GPLv2
+// Copyright 2013 Dolphin Emulator Project / 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 
@@ -676,7 +676,9 @@ const std::string& GetUserPath(const unsigned int DirIDX, const std::string &new
         paths[D_MAPS_IDX]           = paths[D_USER_IDX] + MAPS_DIR DIR_SEP;
         paths[D_CACHE_IDX]          = paths[D_USER_IDX] + CACHE_DIR DIR_SEP;
         paths[D_SDMC_IDX]           = paths[D_USER_IDX] + SDMC_DIR DIR_SEP;
+        paths[D_SAVEDATA_IDX]       = paths[D_USER_IDX] + SAVEDATA_DIR DIR_SEP;
         paths[D_SYSDATA_IDX]        = paths[D_USER_IDX] + SYSDATA_DIR DIR_SEP;
+        paths[D_SYSSAVEDATA_IDX]    = paths[D_USER_IDX] + SYSSAVEDATA_DIR DIR_SEP;
         paths[D_SHADERCACHE_IDX]    = paths[D_USER_IDX] + SHADERCACHE_DIR DIR_SEP;
         paths[D_SHADERS_IDX]        = paths[D_USER_IDX] + SHADERS_DIR DIR_SEP;
         paths[D_STATESAVES_IDX]     = paths[D_USER_IDX] + STATESAVES_DIR DIR_SEP;
@@ -718,6 +720,8 @@ const std::string& GetUserPath(const unsigned int DirIDX, const std::string &new
             paths[D_MAPS_IDX]           = paths[D_USER_IDX] + MAPS_DIR DIR_SEP;
             paths[D_CACHE_IDX]          = paths[D_USER_IDX] + CACHE_DIR DIR_SEP;
             paths[D_SDMC_IDX]           = paths[D_USER_IDX] + SDMC_DIR DIR_SEP;
+            paths[D_SAVEDATA_IDX]       = paths[D_USER_IDX] + SAVEDATA_DIR DIR_SEP;
+            paths[D_SYSSAVEDATA_IDX]    = paths[D_USER_IDX] + SYSSAVEDATA_DIR DIR_SEP;
             paths[D_SHADERCACHE_IDX]    = paths[D_USER_IDX] + SHADERCACHE_DIR DIR_SEP;
             paths[D_SHADERS_IDX]        = paths[D_USER_IDX] + SHADERS_DIR DIR_SEP;
             paths[D_STATESAVES_IDX]     = paths[D_USER_IDX] + STATESAVES_DIR DIR_SEP;
diff --git a/src/common/file_util.h b/src/common/file_util.h
index a9d48cfe8..293c30941 100644
--- a/src/common/file_util.h
+++ b/src/common/file_util.h
@@ -1,5 +1,5 @@
-// Copyright 2013 Dolphin Emulator Project
-// Licensed under GPLv2
+// Copyright 2013 Dolphin Emulator Project / 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -27,7 +27,9 @@ enum {
     D_STATESAVES_IDX,
     D_SCREENSHOTS_IDX,
     D_SDMC_IDX,
+    D_SAVEDATA_IDX,
     D_SYSDATA_IDX,
+    D_SYSSAVEDATA_IDX,
     D_HIRESTEXTURES_IDX,
     D_DUMP_IDX,
     D_DUMPFRAMES_IDX,
diff --git a/src/common/hash.cpp b/src/common/hash.cpp
index 2ddcfe6b7..fe2c9e636 100644
--- a/src/common/hash.cpp
+++ b/src/common/hash.cpp
@@ -1,5 +1,5 @@
-// Copyright 2013 Dolphin Emulator Project
-// Licensed under GPLv2
+// Copyright 2013 Dolphin Emulator Project / 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 
diff --git a/src/common/hash.h b/src/common/hash.h
index 29f699d7f..3ac42bc44 100644
--- a/src/common/hash.h
+++ b/src/common/hash.h
@@ -1,5 +1,5 @@
-// Copyright 2013 Dolphin Emulator Project
-// Licensed under GPLv2
+// Copyright 2013 Dolphin Emulator Project / 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/common/key_map.cpp b/src/common/key_map.cpp
index 309caab98..d8945bb13 100644
--- a/src/common/key_map.cpp
+++ b/src/common/key_map.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "key_map.h"
diff --git a/src/common/key_map.h b/src/common/key_map.h
index bf72362c0..8d949b852 100644
--- a/src/common/key_map.h
+++ b/src/common/key_map.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/common/linear_disk_cache.h b/src/common/linear_disk_cache.h
index bb1b5174f..74ce74aba 100644
--- a/src/common/linear_disk_cache.h
+++ b/src/common/linear_disk_cache.h
@@ -1,5 +1,5 @@
-// Copyright 2013 Dolphin Emulator Project
-// Licensed under GPLv2
+// Copyright 2013 Dolphin Emulator Project / 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/common/log.h b/src/common/log.h
index 663eda9ad..96d97249f 100644
--- a/src/common/log.h
+++ b/src/common/log.h
@@ -1,5 +1,5 @@
-// Copyright 2013 Dolphin Emulator Project
-// Licensed under GPLv2
+// Copyright 2013 Dolphin Emulator Project / 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/common/logging/backend.cpp b/src/common/logging/backend.cpp
index e79b84604..816d1bb55 100644
--- a/src/common/logging/backend.cpp
+++ b/src/common/logging/backend.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2+
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include <algorithm>
diff --git a/src/common/logging/backend.h b/src/common/logging/backend.h
index ae270efe8..1c44c929e 100644
--- a/src/common/logging/backend.h
+++ b/src/common/logging/backend.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2+
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/common/logging/filter.cpp b/src/common/logging/filter.cpp
index 0cf9b05e7..50f2e13f4 100644
--- a/src/common/logging/filter.cpp
+++ b/src/common/logging/filter.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2+
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include <algorithm>
diff --git a/src/common/logging/filter.h b/src/common/logging/filter.h
index 32b14b159..c3da9989f 100644
--- a/src/common/logging/filter.h
+++ b/src/common/logging/filter.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2+
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include <array>
diff --git a/src/common/logging/log.h b/src/common/logging/log.h
index 1eec34fbb..d1c391862 100644
--- a/src/common/logging/log.h
+++ b/src/common/logging/log.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2+
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/common/logging/text_formatter.cpp b/src/common/logging/text_formatter.cpp
index 3fe435346..ef5739d84 100644
--- a/src/common/logging/text_formatter.cpp
+++ b/src/common/logging/text_formatter.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2+
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include <array>
@@ -54,12 +54,22 @@ void FormatLogMessage(const Entry& entry, char* out_text, size_t text_len) {
         TrimSourcePath(entry.location.c_str()), entry.message.c_str());
 }
 
-static void ChangeConsoleColor(Level level) {
+void PrintMessage(const Entry& entry) {
+    std::array<char, 4 * 1024> format_buffer;
+    FormatLogMessage(entry, format_buffer.data(), format_buffer.size());
+    fputs(format_buffer.data(), stderr);
+    fputc('\n', stderr);
+}
+
+void PrintColoredMessage(const Entry& entry) {
 #ifdef _WIN32
     static HANDLE console_handle = GetStdHandle(STD_ERROR_HANDLE);
 
+    CONSOLE_SCREEN_BUFFER_INFO original_info = {0};
+    GetConsoleScreenBufferInfo(console_handle, &original_info);
+
     WORD color = 0;
-    switch (level) {
+    switch (entry.log_level) {
     case Level::Trace: // Grey
         color = FOREGROUND_INTENSITY; break;
     case Level::Debug: // Cyan
@@ -76,9 +86,9 @@ static void ChangeConsoleColor(Level level) {
 
     SetConsoleTextAttribute(console_handle, color);
 #else
-#define ESC "\x1b"
+#   define ESC "\x1b"
     const char* color = "";
-    switch (level) {
+    switch (entry.log_level) {
     case Level::Trace: // Grey
         color = ESC "[1;30m"; break;
     case Level::Debug: // Cyan
@@ -92,18 +102,18 @@ static void ChangeConsoleColor(Level level) {
     case Level::Critical: // Bright magenta
         color = ESC "[1;35m"; break;
     }
-#undef ESC
 
     fputs(color, stderr);
 #endif
-}
 
-void PrintMessage(const Entry& entry) {
-    ChangeConsoleColor(entry.log_level);
-    std::array<char, 4 * 1024> format_buffer;
-    FormatLogMessage(entry, format_buffer.data(), format_buffer.size());
-    fputs(format_buffer.data(), stderr);
-    fputc('\n', stderr);
+    PrintMessage(entry);
+
+#ifdef _WIN32
+    SetConsoleTextAttribute(console_handle, original_info.wAttributes);
+#else
+    fputs(ESC "[0m", stderr);
+#   undef ESC
+#endif
 }
 
 void TextLoggingLoop(std::shared_ptr<Logger> logger, const Filter* filter) {
@@ -117,7 +127,7 @@ void TextLoggingLoop(std::shared_ptr<Logger> logger, const Filter* filter) {
         for (size_t i = 0; i < num_entries; ++i) {
             const Entry& entry = entry_buffer[i];
             if (filter->CheckMessage(entry.log_class, entry.log_level)) {
-                PrintMessage(entry);
+                PrintColoredMessage(entry);
             }
         }
     }
diff --git a/src/common/logging/text_formatter.h b/src/common/logging/text_formatter.h
index d7e298e28..2f05794f0 100644
--- a/src/common/logging/text_formatter.h
+++ b/src/common/logging/text_formatter.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2+
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -29,6 +29,8 @@ const char* TrimSourcePath(const char* path, const char* root = "src");
 void FormatLogMessage(const Entry& entry, char* out_text, size_t text_len);
 /// Formats and prints a log entry to stderr.
 void PrintMessage(const Entry& entry);
+/// Prints the same message as `PrintMessage`, but colored acoording to the severity level.
+void PrintColoredMessage(const Entry& entry);
 
 /**
  * Logging loop that repeatedly reads messages from the provided logger and prints them to the
diff --git a/src/common/make_unique.h b/src/common/make_unique.h
new file mode 100644
index 000000000..2a7b76412
--- /dev/null
+++ b/src/common/make_unique.h
@@ -0,0 +1,16 @@
+// Copyright 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#pragma once
+
+#include <memory>
+
+namespace Common {
+
+template <typename T, typename... Args>
+std::unique_ptr<T> make_unique(Args&&... args) {
+    return std::unique_ptr<T>(new T(std::forward<Args>(args)...));
+}
+
+} // namespace
diff --git a/src/common/math_util.cpp b/src/common/math_util.cpp
index 3613e82a6..a83592dd2 100644
--- a/src/common/math_util.cpp
+++ b/src/common/math_util.cpp
@@ -1,5 +1,5 @@
-// Copyright 2013 Dolphin Emulator Project
-// Licensed under GPLv2
+// Copyright 2013 Dolphin Emulator Project / 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 
diff --git a/src/common/math_util.h b/src/common/math_util.h
index b10a25c13..43b0e0dc3 100644
--- a/src/common/math_util.h
+++ b/src/common/math_util.h
@@ -1,5 +1,5 @@
-// Copyright 2013 Dolphin Emulator Project
-// Licensed under GPLv2
+// Copyright 2013 Dolphin Emulator Project / 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/common/memory_util.cpp b/src/common/memory_util.cpp
index ca8a2a9e4..8f982da89 100644
--- a/src/common/memory_util.cpp
+++ b/src/common/memory_util.cpp
@@ -1,5 +1,5 @@
-// Copyright 2013 Dolphin Emulator Project
-// Licensed under GPLv2
+// Copyright 2013 Dolphin Emulator Project / 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 
diff --git a/src/common/memory_util.h b/src/common/memory_util.h
index 922bd44b2..9fdbf1f12 100644
--- a/src/common/memory_util.h
+++ b/src/common/memory_util.h
@@ -1,5 +1,5 @@
-// Copyright 2013 Dolphin Emulator Project
-// Licensed under GPLv2
+// Copyright 2013 Dolphin Emulator Project / 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/common/misc.cpp b/src/common/misc.cpp
index bc9d26188..e33055d10 100644
--- a/src/common/misc.cpp
+++ b/src/common/misc.cpp
@@ -1,5 +1,5 @@
-// Copyright 2013 Dolphin Emulator Project
-// Licensed under GPLv2
+// Copyright 2013 Dolphin Emulator Project / 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "common/common.h"
diff --git a/src/common/msg_handler.cpp b/src/common/msg_handler.cpp
index 7ffedc45a..4a47b518e 100644
--- a/src/common/msg_handler.cpp
+++ b/src/common/msg_handler.cpp
@@ -1,5 +1,5 @@
-// Copyright 2013 Dolphin Emulator Project
-// Licensed under GPLv2
+// Copyright 2013 Dolphin Emulator Project / 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include <cstdio>
diff --git a/src/common/msg_handler.h b/src/common/msg_handler.h
index 9bfdf950e..7bb216e98 100644
--- a/src/common/msg_handler.h
+++ b/src/common/msg_handler.h
@@ -1,5 +1,5 @@
-// Copyright 2013 Dolphin Emulator Project
-// Licensed under GPLv2
+// Copyright 2013 Dolphin Emulator Project / 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/common/scm_rev.h b/src/common/scm_rev.h
index d34664614..0ef190afa 100644
--- a/src/common/scm_rev.h
+++ b/src/common/scm_rev.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/common/scope_exit.h b/src/common/scope_exit.h
index 1d3e59319..263beaf0e 100644
--- a/src/common/scope_exit.h
+++ b/src/common/scope_exit.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2+
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/common/string_util.cpp b/src/common/string_util.cpp
index 6d9612fb5..d919b7a4c 100644
--- a/src/common/string_util.cpp
+++ b/src/common/string_util.cpp
@@ -1,5 +1,5 @@
-// Copyright 2013 Dolphin Emulator Project
-// Licensed under GPLv2
+// Copyright 2013 Dolphin Emulator Project / 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include <boost/range/algorithm.hpp>
diff --git a/src/common/string_util.h b/src/common/string_util.h
index 7d75691b1..74974263f 100644
--- a/src/common/string_util.h
+++ b/src/common/string_util.h
@@ -1,5 +1,5 @@
-// Copyright 2013 Dolphin Emulator Project
-// Licensed under GPLv2
+// Copyright 2013 Dolphin Emulator Project / 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/common/symbols.cpp b/src/common/symbols.cpp
index 63ad6218b..9e4dccfb3 100644
--- a/src/common/symbols.cpp
+++ b/src/common/symbols.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "common/symbols.h"
diff --git a/src/common/symbols.h b/src/common/symbols.h
index 4560f5240..f76cb6b1e 100644
--- a/src/common/symbols.h
+++ b/src/common/symbols.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/common/thread.cpp b/src/common/thread.cpp
index dc153ba71..8c83d67b5 100644
--- a/src/common/thread.cpp
+++ b/src/common/thread.cpp
@@ -1,5 +1,5 @@
-// Copyright 2013 Dolphin Emulator Project
-// Licensed under GPLv2
+// Copyright 2013 Dolphin Emulator Project / 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "common/thread.h"
diff --git a/src/common/thread.h b/src/common/thread.h
index 8c36d3f07..eaf1ba00c 100644
--- a/src/common/thread.h
+++ b/src/common/thread.h
@@ -1,5 +1,5 @@
-// Copyright 2013 Dolphin Emulator Project
-// Licensed under GPLv2
+// Copyright 2013 Dolphin Emulator Project / 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/common/thread_queue_list.h b/src/common/thread_queue_list.h
index 7e3b620c7..4e1c0a215 100644
--- a/src/common/thread_queue_list.h
+++ b/src/common/thread_queue_list.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project / PPSSPP Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/common/thunk.h b/src/common/thunk.h
index 90c8be888..4fb7c98e1 100644
--- a/src/common/thunk.h
+++ b/src/common/thunk.h
@@ -1,5 +1,5 @@
-// Copyright 2013 Dolphin Emulator Project
-// Licensed under GPLv2
+// Copyright 2013 Dolphin Emulator Project / 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/common/timer.cpp b/src/common/timer.cpp
index 4a797f751..a6682ea19 100644
--- a/src/common/timer.cpp
+++ b/src/common/timer.cpp
@@ -1,5 +1,5 @@
-// Copyright 2013 Dolphin Emulator Project
-// Licensed under GPLv2
+// Copyright 2013 Dolphin Emulator Project / 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include <time.h>
diff --git a/src/common/timer.h b/src/common/timer.h
index 86418e7a7..4b44c33a0 100644
--- a/src/common/timer.h
+++ b/src/common/timer.h
@@ -1,5 +1,5 @@
-// Copyright 2013 Dolphin Emulator Project
-// Licensed under GPLv2
+// Copyright 2013 Dolphin Emulator Project / 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/core/CMakeLists.txt b/src/core/CMakeLists.txt
index 8f6792791..fdd97c184 100644
--- a/src/core/CMakeLists.txt
+++ b/src/core/CMakeLists.txt
@@ -18,35 +18,46 @@ set(SRCS
             arm/skyeye_common/vfp/vfpinstr.cpp
             arm/skyeye_common/vfp/vfpsingle.cpp
             file_sys/archive_romfs.cpp
+            file_sys/archive_savedata.cpp
             file_sys/archive_sdmc.cpp
+            file_sys/archive_systemsavedata.cpp
+            file_sys/disk_archive.cpp
             file_sys/file_romfs.cpp
-            file_sys/file_sdmc.cpp
             file_sys/directory_romfs.cpp
-            file_sys/directory_sdmc.cpp
             hle/kernel/address_arbiter.cpp
-            hle/kernel/archive.cpp
             hle/kernel/event.cpp
             hle/kernel/kernel.cpp
             hle/kernel/mutex.cpp
+            hle/kernel/semaphore.cpp
             hle/kernel/shared_memory.cpp
             hle/kernel/thread.cpp
             hle/service/ac_u.cpp
+            hle/service/act_u.cpp
+            hle/service/am_app.cpp
             hle/service/am_net.cpp
+            hle/service/apt_a.cpp
             hle/service/apt_u.cpp
             hle/service/boss_u.cpp
-            hle/service/cfg_i.cpp
-            hle/service/cfg_u.cpp
+            hle/service/cecd_u.cpp
+            hle/service/cfg/cfg.cpp
+            hle/service/cfg/cfg_i.cpp
+            hle/service/cfg/cfg_u.cpp
             hle/service/csnd_snd.cpp
             hle/service/dsp_dsp.cpp
             hle/service/err_f.cpp
-            hle/service/fs_user.cpp
             hle/service/frd_u.cpp
+            hle/service/fs/archive.cpp
+            hle/service/fs/fs_user.cpp
             hle/service/gsp_gpu.cpp
             hle/service/hid_user.cpp
+            hle/service/http_c.cpp
             hle/service/ir_rst.cpp
             hle/service/ir_u.cpp
+            hle/service/ldr_ro.cpp
             hle/service/mic_u.cpp
             hle/service/ndm_u.cpp
+            hle/service/news_u.cpp
+            hle/service/nim_aoc.cpp
             hle/service/nwm_uds.cpp
             hle/service/pm_app.cpp
             hle/service/ptm_u.cpp
@@ -92,39 +103,51 @@ set(HEADERS
             arm/skyeye_common/vfp/vfp.h
             arm/skyeye_common/vfp/vfp_helper.h
             arm/arm_interface.h
-            file_sys/archive.h
+            file_sys/archive_backend.h
             file_sys/archive_romfs.h
+            file_sys/archive_savedata.h
             file_sys/archive_sdmc.h
-            file_sys/file.h
+            file_sys/archive_systemsavedata.h
+            file_sys/disk_archive.h
+            file_sys/file_backend.h
             file_sys/file_romfs.h
-            file_sys/file_sdmc.h
-            file_sys/directory.h
+            file_sys/directory_backend.h
             file_sys/directory_romfs.h
-            file_sys/directory_sdmc.h
             hle/kernel/address_arbiter.h
-            hle/kernel/archive.h
             hle/kernel/event.h
             hle/kernel/kernel.h
             hle/kernel/mutex.h
+            hle/kernel/semaphore.h
+            hle/kernel/session.h
             hle/kernel/shared_memory.h
             hle/kernel/thread.h
             hle/service/ac_u.h
+            hle/service/act_u.h
+            hle/service/am_app.h
             hle/service/am_net.h
+            hle/service/apt_a.h
             hle/service/apt_u.h
             hle/service/boss_u.h
-            hle/service/cfg_i.h
-            hle/service/cfg_u.h
+            hle/service/cecd_u.h
+            hle/service/cfg/cfg.h
+            hle/service/cfg/cfg_i.h
+            hle/service/cfg/cfg_u.h
             hle/service/csnd_snd.h
             hle/service/dsp_dsp.h
             hle/service/err_f.h
-            hle/service/fs_user.h
             hle/service/frd_u.h
+            hle/service/fs/archive.h
+            hle/service/fs/fs_user.h
             hle/service/gsp_gpu.h
             hle/service/hid_user.h
+            hle/service/http_c.h
             hle/service/ir_rst.h
             hle/service/ir_u.h
+            hle/service/ldr_ro.h
             hle/service/mic_u.h
             hle/service/ndm_u.h
+            hle/service/news_u.h
+            hle/service/nim_aoc.h
             hle/service/nwm_uds.h
             hle/service/pm_app.h
             hle/service/ptm_u.h
diff --git a/src/core/arm/arm_interface.h b/src/core/arm/arm_interface.h
index 3ae528562..3b7209418 100644
--- a/src/core/arm/arm_interface.h
+++ b/src/core/arm/arm_interface.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -78,6 +78,12 @@ public:
     virtual u64 GetTicks() const = 0;
 
     /**
+     * Advance the CPU core by the specified number of ticks (e.g. to simulate CPU execution time)
+     * @param ticks Number of ticks to advance the CPU core
+     */
+    virtual void AddTicks(u64 ticks) = 0;
+
+    /**
      * Saves the current CPU context
      * @param ctx Thread context to save
      */
diff --git a/src/core/arm/disassembler/load_symbol_map.cpp b/src/core/arm/disassembler/load_symbol_map.cpp
index 55278474b..13d26d170 100644
--- a/src/core/arm/disassembler/load_symbol_map.cpp
+++ b/src/core/arm/disassembler/load_symbol_map.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include <string>
diff --git a/src/core/arm/disassembler/load_symbol_map.h b/src/core/arm/disassembler/load_symbol_map.h
index 837cca99b..d28c551c3 100644
--- a/src/core/arm/disassembler/load_symbol_map.h
+++ b/src/core/arm/disassembler/load_symbol_map.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/core/arm/dyncom/arm_dyncom.cpp b/src/core/arm/dyncom/arm_dyncom.cpp
index 6c8ea211e..a838fd25a 100644
--- a/src/core/arm/dyncom/arm_dyncom.cpp
+++ b/src/core/arm/dyncom/arm_dyncom.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "core/arm/skyeye_common/armcpu.h"
@@ -47,68 +47,38 @@ ARM_DynCom::ARM_DynCom() : ticks(0) {
 ARM_DynCom::~ARM_DynCom() {
 }
 
-/**
- * Set the Program Counter to an address
- * @param addr Address to set PC to
- */
 void ARM_DynCom::SetPC(u32 pc) {
     state->pc = state->Reg[15] = pc;
 }
 
-/*
- * Get the current Program Counter
- * @return Returns current PC
- */
 u32 ARM_DynCom::GetPC() const {
     return state->Reg[15];
 }
 
-/**
- * Get an ARM register
- * @param index Register index (0-15)
- * @return Returns the value in the register
- */
 u32 ARM_DynCom::GetReg(int index) const {
     return state->Reg[index];
 }
 
-/**
- * Set an ARM register
- * @param index Register index (0-15)
- * @param value Value to set register to
- */
 void ARM_DynCom::SetReg(int index, u32 value) {
     state->Reg[index] = value;
 }
 
-/**
- * Get the current CPSR register
- * @return Returns the value of the CPSR register
- */
 u32 ARM_DynCom::GetCPSR() const {
     return state->Cpsr;
 }
 
-/**
- * Set the current CPSR register
- * @param cpsr Value to set CPSR to
- */
 void ARM_DynCom::SetCPSR(u32 cpsr) {
     state->Cpsr = cpsr;
 }
 
-/**
- * Returns the number of clock ticks since the last reset
- * @return Returns number of clock ticks
- */
 u64 ARM_DynCom::GetTicks() const {
     return ticks;
 }
 
-/**
- * Executes the given number of instructions
- * @param num_instructions Number of instructions to executes
- */
+void ARM_DynCom::AddTicks(u64 ticks) {
+    this->ticks += ticks;
+}
+
 void ARM_DynCom::ExecuteInstructions(int num_instructions) {
     state->NumInstrsToExecute = num_instructions;
 
@@ -118,11 +88,6 @@ void ARM_DynCom::ExecuteInstructions(int num_instructions) {
     ticks += InterpreterMainLoop(state.get());
 }
 
-/**
- * Saves the current CPU context
- * @param ctx Thread context to save
- * @todo Do we need to save Reg[15] and NextInstr?
- */
 void ARM_DynCom::SaveContext(ThreadContext& ctx) {
     memcpy(ctx.cpu_registers, state->Reg, sizeof(ctx.cpu_registers));
     memcpy(ctx.fpu_registers, state->ExtReg, sizeof(ctx.fpu_registers));
@@ -139,11 +104,6 @@ void ARM_DynCom::SaveContext(ThreadContext& ctx) {
     ctx.mode = state->NextInstr;
 }
 
-/**
- * Loads a CPU context
- * @param ctx Thread context to load
- * @param Do we need to load Reg[15] and NextInstr?
- */
 void ARM_DynCom::LoadContext(const ThreadContext& ctx) {
     memcpy(state->Reg, ctx.cpu_registers, sizeof(ctx.cpu_registers));
     memcpy(state->ExtReg, ctx.fpu_registers, sizeof(ctx.fpu_registers));
@@ -160,7 +120,6 @@ void ARM_DynCom::LoadContext(const ThreadContext& ctx) {
     state->NextInstr = ctx.mode;
 }
 
-/// Prepare core for thread reschedule (if needed to correctly handle state)
 void ARM_DynCom::PrepareReschedule() {
     state->NumInstrsToExecute = 0;
 }
diff --git a/src/core/arm/dyncom/arm_dyncom.h b/src/core/arm/dyncom/arm_dyncom.h
index 51eea41ed..7284dcd07 100644
--- a/src/core/arm/dyncom/arm_dyncom.h
+++ b/src/core/arm/dyncom/arm_dyncom.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -27,14 +27,14 @@ public:
      * Get the current Program Counter
      * @return Returns current PC
      */
-    u32 GetPC() const;
+    u32 GetPC() const override;
 
     /**
      * Get an ARM register
      * @param index Register index (0-15)
      * @return Returns the value in the register
      */
-    u32 GetReg(int index) const;
+    u32 GetReg(int index) const override;
 
     /**
      * Set an ARM register
@@ -47,7 +47,7 @@ public:
      * Get the current CPSR register
      * @return Returns the value of the CPSR register
      */
-    u32 GetCPSR() const;
+    u32 GetCPSR() const override;
 
     /**
      * Set the current CPSR register
@@ -59,7 +59,13 @@ public:
      * Returns the number of clock ticks since the last reset
      * @return Returns number of clock ticks
      */
-    u64 GetTicks() const;
+    u64 GetTicks() const override;
+
+    /**
+    * Advance the CPU core by the specified number of ticks (e.g. to simulate CPU execution time)
+    * @param ticks Number of ticks to advance the CPU core
+    */
+    void AddTicks(u64 ticks) override;
 
     /**
      * Saves the current CPU context
diff --git a/src/core/arm/dyncom/arm_dyncom_dec.cpp b/src/core/arm/dyncom/arm_dyncom_dec.cpp
index 5d174a08f..333b40f54 100644
--- a/src/core/arm/dyncom/arm_dyncom_dec.cpp
+++ b/src/core/arm/dyncom/arm_dyncom_dec.cpp
@@ -1,402 +1,443 @@
-/* Copyright (C) 
-* 2012 - Michael.Kang blackfin.kang@gmail.com
-* This program is free software; you can redistribute it and/or
-* modify it under the terms of the GNU General Public License
-* as published by the Free Software Foundation; either version 2
-* of the License, or (at your option) any later version.
-* 
-* This program is distributed in the hope that 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, write to the Free Software
-* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
-* 
-*/
-/**
-* @file arm_dyncom_dec.cpp
-* @brief Some common utility for arm decoder
-* @author Michael.Kang blackfin.kang@gmail.com
-* @version 7849
-* @date 2012-03-15
-*/
+// Copyright 2012 Michael Kang, 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
 
 #include "core/arm/skyeye_common/arm_regformat.h"
 #include "core/arm/skyeye_common/armdefs.h"
 #include "core/arm/dyncom/arm_dyncom_dec.h"
 
 const ISEITEM arm_instruction[] = {
-        #define VFP_DECODE
-        #include "core/arm/skyeye_common/vfp/vfpinstr.cpp"
-        #undef VFP_DECODE
-        {"srs"  ,  4    ,  6    , 25, 31, 0x0000007c, 22, 22, 0x00000001, 16, 20, 0x0000000d,  8, 11, 0x00000005},
-        {"rfe"  ,  4    ,  6    , 25, 31, 0x0000007c, 22, 22, 0x00000000, 20, 20, 0x00000001,  8, 11, 0x0000000a},
-        {"bkpt" ,  2    ,  3    , 20, 31, 0x00000e12,  4,  7, 0x00000007},
-        {"blx"  ,  1    ,  3    , 25, 31, 0x0000007d},
-        {"cps"  ,  3    ,  6    , 20, 31, 0x00000f10, 16, 16, 0x00000000,  5,  5, 0x00000000},
-        {"pld"  ,  4    ,  4    , 26, 31, 0x0000003d, 24, 24, 0x00000001, 20, 22, 0x00000005, 12, 15, 0x0000000f},
-        {"setend"       ,  2    ,  6    , 16, 31, 0x0000f101,  4,  7, 0x00000000},
-        {"clrex"        ,  1    ,  6    ,  0, 31, 0xf57ff01f},
-        {"rev16"        ,  2    ,  6    , 16, 27, 0x000006bf,  4, 11, 0x000000fb},
-        {"usad8"        ,  3    ,  6    , 20, 27, 0x00000078, 12, 15, 0x0000000f,  4,  7, 0x00000001},
-        {"sxtb" ,  2    ,  6    , 16, 27, 0x000006af,  4,  7, 0x00000007},
-        {"uxtb" ,  2    ,  6    , 16, 27, 0x000006ef,  4,  7, 0x00000007},
-        {"sxth" ,  2    ,  6    , 16, 27, 0x000006bf,  4,  7, 0x00000007},
-        {"sxtb16"       ,  2    ,  6    , 16, 27, 0x0000068f,  4,  7, 0x00000007},
-        {"uxth" ,  2    ,  6    , 16, 27, 0x000006ff,  4,  7, 0x00000007},
-        {"uxtb16"       ,  2    ,  6    , 16, 27, 0x000006cf,  4,  7, 0x00000007},
-        {"cpy"  ,  2    ,  6    , 20, 27, 0x0000001a,  4, 11, 0x00000000},
-        {"uxtab"        ,  2    ,  6    , 20, 27, 0x0000006e,  4,  9, 0x00000007},
-        {"ssub8"        ,  2    ,  6    , 20, 27, 0x00000061,  4,  7, 0x0000000f},
-        {"shsub8"       ,  2    ,  6    , 20, 27, 0x00000063,  4,  7, 0x0000000f},
-        {"ssubaddx"     ,  2    ,  6    , 20, 27, 0x00000061,  4,  7, 0x00000005},
-        {"strex"        ,  2    ,  6    , 20, 27, 0x00000018,  4,  7, 0x00000009},
-        {"strexb"       ,  2    ,  7    , 20, 27, 0x0000001c,  4,  7, 0x00000009},
-        {"swp"  ,  2    ,  0    , 20, 27, 0x00000010,  4,  7, 0x00000009},
-        {"swpb" ,  2    ,  0    , 20, 27, 0x00000014,  4,  7, 0x00000009},
-        {"ssub16"       ,  2    ,  6    , 20, 27, 0x00000061,  4,  7, 0x00000007},
-        {"ssat16"       ,  2    ,  6    , 20, 27, 0x0000006a,  4,  7, 0x00000003},
-        {"shsubaddx"    ,  2    ,  6    , 20, 27, 0x00000063,  4,  7, 0x00000005},
-        {"qsubaddx"     ,  2    ,  6    , 20, 27, 0x00000062,  4,  7, 0x00000005},
-        {"shaddsubx"    ,  2    ,  6    , 20, 27, 0x00000063,  4,  7, 0x00000003},
-        {"shadd8"       ,  2    ,  6    , 20, 27, 0x00000063,  4,  7, 0x00000009},
-        {"shadd16"      ,  2    ,  6    , 20, 27, 0x00000063,  4,  7, 0x00000001},
-        {"sel"  ,  2    ,  6    , 20, 27, 0x00000068,  4,  7, 0x0000000b},
-        {"saddsubx"     ,  2    ,  6    , 20, 27, 0x00000061,  4,  7, 0x00000003},
-        {"sadd8"        ,  2    ,  6    , 20, 27, 0x00000061,  4,  7, 0x00000009},
-        {"sadd16"       ,  2    ,  6    , 20, 27, 0x00000061,  4,  7, 0x00000001},
-        {"shsub16"      ,  2    ,  6    , 20, 27, 0x00000063,  4,  7, 0x00000007},
-        {"umaal"        ,  2    ,  6    , 20, 27, 0x00000004,  4,  7, 0x00000009},
-        {"uxtab16"      ,  2    ,  6    , 20, 27, 0x0000006c,  4,  7, 0x00000007},
-        {"usubaddx"     ,  2    ,  6    , 20, 27, 0x00000065,  4,  7, 0x00000005},
-        {"usub8"        ,  2    ,  6    , 20, 27, 0x00000065,  4,  7, 0x0000000f},
-        {"usub16"       ,  2    ,  6    , 20, 27, 0x00000065,  4,  7, 0x00000007},
-        {"usat16"       ,  2    ,  6    , 20, 27, 0x0000006e,  4,  7, 0x00000003},
-        {"usada8"       ,  2    ,  6    , 20, 27, 0x00000078,  4,  7, 0x00000001},
-        {"uqsubaddx"    ,  2    ,  6    , 20, 27, 0x00000066,  4,  7, 0x00000005},
-        {"uqsub8"       ,  2    ,  6    , 20, 27, 0x00000066,  4,  7, 0x0000000f},
-        {"uqsub16"      ,  2    ,  6    , 20, 27, 0x00000066,  4,  7, 0x00000007},
-        {"uqaddsubx"    ,  2    ,  6    , 20, 27, 0x00000066,  4,  7, 0x00000003},
-        {"uqadd8"       ,  2    ,  6    , 20, 27, 0x00000066,  4,  7, 0x00000009},
-        {"uqadd16"      ,  2    ,  6    , 20, 27, 0x00000066,  4,  7, 0x00000001},
-        {"sxtab"        ,  2    ,  6    , 20, 27, 0x0000006a,  4,  7, 0x00000007},
-        {"uhsubaddx"    ,  2    ,  6    , 20, 27, 0x00000067,  4,  7, 0x00000005},
-        {"uhsub8"       ,  2    ,  6    , 20, 27, 0x00000067,  4,  7, 0x0000000f},
-        {"uhsub16"      ,  2    ,  6    , 20, 27, 0x00000067,  4,  7, 0x00000007},
-        {"uhaddsubx"    ,  2    ,  6    , 20, 27, 0x00000067,  4,  7, 0x00000003},
-        {"uhadd8"       ,  2    ,  6    , 20, 27, 0x00000067,  4,  7, 0x00000009},
-        {"uhadd16"      ,  2    ,  6    , 20, 27, 0x00000067,  4,  7, 0x00000001},
-        {"uaddsubx"     ,  2    ,  6    , 20, 27, 0x00000065,  4,  7, 0x00000003},
-        {"uadd8"        ,  2    ,  6    , 20, 27, 0x00000065,  4,  7, 0x00000009},
-        {"uadd16"       ,  2    ,  6    , 20, 27, 0x00000065,  4,  7, 0x00000001},
-        {"sxtah"        ,  2    ,  6    , 20, 27, 0x0000006b,  4,  7, 0x00000007},
-        {"sxtab16"      ,  2    ,  6    , 20, 27, 0x00000068,  4,  7, 0x00000007},
-        {"qadd8"        ,  2    ,  6    , 20, 27, 0x00000062,  4,  7, 0x00000009},
-        {"bxj"  ,  2    ,  5    , 20, 27, 0x00000012,  4,  7, 0x00000002},
-        {"clz"  ,  2    ,  3    , 20, 27, 0x00000016,  4,  7, 0x00000001},
-        {"uxtah"        ,  2    ,  6    , 20, 27, 0x0000006f,  4,  7, 0x00000007},
-        {"bx"   ,  2    ,  2    , 20, 27, 0x00000012,  4,  7, 0x00000001},
-        {"rev"  ,  2    ,  6    , 20, 27, 0x0000006b,  4,  7, 0x00000003},
-        {"blx"  ,  2    ,  3    , 20, 27, 0x00000012,  4,  7, 0x00000003},
-        {"revsh"        ,  2    ,  6    , 20, 27, 0x0000006f,  4,  7, 0x0000000b},
-        {"qadd" ,  2    ,  4    , 20, 27, 0x00000010,  4,  7, 0x00000005},
-        {"qadd16"       ,  2    ,  6    , 20, 27, 0x00000062,  4,  7, 0x00000001},
-        {"qaddsubx"     ,  2    ,  6    , 20, 27, 0x00000062,  4,  7, 0x00000003},
-        {"ldrex"        ,  2    ,  0    , 20, 27, 0x00000019,  4,  7, 0x00000009},
-        {"qdadd"        ,  2    ,  4    , 20, 27, 0x00000014,  4,  7, 0x00000005},
-        {"qdsub"        ,  2    ,  4    , 20, 27, 0x00000016,  4,  7, 0x00000005},
-        {"qsub" ,  2    ,  4    , 20, 27, 0x00000012,  4,  7, 0x00000005},
-        {"ldrexb"       ,  2    ,  7    , 20, 27, 0x0000001d,  4,  7, 0x00000009},
-        {"qsub8"        ,  2    ,  6    , 20, 27, 0x00000062,  4,  7, 0x0000000f},
-        {"qsub16"       ,  2    ,  6    , 20, 27, 0x00000062,  4,  7, 0x00000007},
-        {"smuad"        ,  4    ,  6    , 20, 27, 0x00000070, 12, 15, 0x0000000f,  6,  7, 0x00000000,  4,  4, 0x00000001},
-        {"smmul"        ,  4    ,  6    , 20, 27, 0x00000075, 12, 15, 0x0000000f,  6,  7, 0x00000000,  4,  4, 0x00000001},
-        {"smusd"        ,  4    ,  6    , 20, 27, 0x00000070, 12, 15, 0x0000000f,  6,  7, 0x00000001,  4,  4, 0x00000001},
-        {"smlsd"        ,  3    ,  6    , 20, 27, 0x00000070,  6,  7, 0x00000001,  4,  4, 0x00000001},
-        {"smlsld"       ,  3    ,  6    , 20, 27, 0x00000074,  6,  7, 0x00000001,  4,  4, 0x00000001},
-        {"smmla"        ,  3    ,  6    , 20, 27, 0x00000075,  6,  7, 0x00000000,  4,  4, 0x00000001},
-        {"smmls"        ,  3    ,  6    , 20, 27, 0x00000075,  6,  7, 0x00000003,  4,  4, 0x00000001},
-        {"smlald"       ,  3    ,  6    , 20, 27, 0x00000074,  6,  7, 0x00000000,  4,  4, 0x00000001},
-        {"smlad"        ,  3    ,  6    , 20, 27, 0x00000070,  6,  7, 0x00000000,  4,  4, 0x00000001},
-        {"smlaw"        ,  3    ,  4    , 20, 27, 0x00000012,  7,  7, 0x00000001,  4,  5, 0x00000000},
-        {"smulw"        ,  3    ,  4    , 20, 27, 0x00000012,  7,  7, 0x00000001,  4,  5, 0x00000002},
-        {"pkhtb"        ,  2    ,  6    , 20, 27, 0x00000068,  4,  6, 0x00000005},
-        {"pkhbt"        ,  2    ,  6    , 20, 27, 0x00000068,  4,  6, 0x00000001},
-        {"smul" ,  3    ,  4    , 20, 27, 0x00000016,  7,  7, 0x00000001,  4,  4, 0x00000000},
-        {"smlalxy"      ,  3    ,  4    , 20, 27, 0x00000014,  7,  7, 0x00000001,  4,  4, 0x00000000},
-//      {"smlal"        ,  2    ,  4    , 21, 27, 0x00000007,  4,  7, 0x00000009},
-        {"smla" ,  3    ,  4    , 20, 27, 0x00000010,  7,  7, 0x00000001,  4,  4, 0x00000000},
-        {"mcrr" ,  1    ,  6    , 20, 27, 0x000000c4},
-        {"mrrc" ,  1    ,  6    , 20, 27, 0x000000c5},
-        {"cmp"  ,  2    ,  0    , 26, 27, 0x00000000, 20, 24, 0x00000015},
-        {"tst"  ,  2    ,  0    , 26, 27, 0x00000000, 20, 24, 0x00000011},
-        {"teq"  ,  2    ,  0    , 26, 27, 0x00000000, 20, 24, 0x00000013},
-        {"cmn"  ,  2    ,  0    , 26, 27, 0x00000000, 20, 24, 0x00000017},
-        {"smull"        ,  2    ,  0    , 21, 27, 0x00000006,  4,  7, 0x00000009},
-        {"umull"        ,  2    ,  0    , 21, 27, 0x00000004,  4,  7, 0x00000009},
-        {"umlal"        ,  2    ,  0    , 21, 27, 0x00000005,  4,  7, 0x00000009},
-        {"smlal"        ,  2    ,  0    , 21, 27, 0x00000007,  4,  7, 0x00000009},
-        {"mul"  ,  2    ,  0    , 21, 27, 0x00000000,  4,  7, 0x00000009},
-        {"mla"  ,  2    ,  0    , 21, 27, 0x00000001,  4,  7, 0x00000009},
-        {"ssat" ,  2    ,  6    , 21, 27, 0x00000035,  4,  5, 0x00000001},
-        {"usat" ,  2    ,  6    , 21, 27, 0x00000037,  4,  5, 0x00000001},
-        {"mrs"  ,  4    ,  0    , 23, 27, 0x00000002, 20, 21, 0x00000000, 16, 19, 0x0000000f,  0, 11, 0x00000000},
-        {"msr"  ,  3    ,  0    , 23, 27, 0x00000002, 20, 21, 0x00000002,  4,  7, 0x00000000},
-        {"and"  ,  2    ,  0    , 26, 27, 0x00000000, 21, 24, 0x00000000},
-        {"bic"  ,  2    ,  0    , 26, 27, 0x00000000, 21, 24, 0x0000000e},
-        {"ldm"  ,  3    ,  0    , 25, 27, 0x00000004, 20, 22, 0x00000005, 15, 15, 0x00000000},
-        {"eor"  ,  2    ,  0    , 26, 27, 0x00000000, 21, 24, 0x00000001},
-        {"add"  ,  2    ,  0    , 26, 27, 0x00000000, 21, 24, 0x00000004},
-        {"rsb"  ,  2    ,  0    , 26, 27, 0x00000000, 21, 24, 0x00000003},
-        {"rsc"  ,  2    ,  0    , 26, 27, 0x00000000, 21, 24, 0x00000007},
-        {"sbc"  ,  2    ,  0    , 26, 27, 0x00000000, 21, 24, 0x00000006},
-        {"adc"  ,  2    ,  0    , 26, 27, 0x00000000, 21, 24, 0x00000005},
-        {"sub"  ,  2    ,  0    , 26, 27, 0x00000000, 21, 24, 0x00000002},
-        {"orr"  ,  2    ,  0    , 26, 27, 0x00000000, 21, 24, 0x0000000c},
-        {"mvn"  ,  2    ,  0    , 26, 27, 0x00000000, 21, 24, 0x0000000f},
-        {"mov"  ,  2    ,  0    , 26, 27, 0x00000000, 21, 24, 0x0000000d},
-        {"stm"  ,  2    ,  0    , 25, 27, 0x00000004, 20, 22, 0x00000004},
-        {"ldm"  ,  4    ,  0    , 25, 27, 0x00000004, 22, 22, 0x00000001, 20, 20, 0x00000001, 15, 15, 0x00000001},
-        {"ldrsh"        ,  3    ,  2    , 25, 27, 0x00000000, 20, 20, 0x00000001,  4,  7, 0x0000000f},
-        {"stm"  ,  3    ,  0    , 25, 27, 0x00000004, 22, 22, 0x00000000, 20, 20, 0x00000000},
-        {"ldm"  ,  3    ,  0    , 25, 27, 0x00000004, 22, 22, 0x00000000, 20, 20, 0x00000001},
-        {"ldrsb"        ,  3    ,  2    , 25, 27, 0x00000000, 20, 20, 0x00000001,  4,  7, 0x0000000d},
-        {"strd" ,  3    ,  4    , 25, 27, 0x00000000, 20, 20, 0x00000000,  4,  7, 0x0000000f},
-        {"ldrh" ,  3    ,  0    , 25, 27, 0x00000000, 20, 20, 0x00000001,  4,  7, 0x0000000b},
-        {"strh" ,  3    ,  0    , 25, 27, 0x00000000, 20, 20, 0x00000000,  4,  7, 0x0000000b},
-        {"ldrd" ,  3    ,  4    , 25, 27, 0x00000000, 20, 20, 0x00000000,  4,  7, 0x0000000d},
-        {"strt" ,  3    ,  0    , 26, 27, 0x00000001, 24, 24, 0x00000000, 20, 22, 0x00000002},
-        {"strbt"        ,  3    ,  0    , 26, 27, 0x00000001, 24, 24, 0x00000000, 20, 22, 0x00000006},
-        {"ldrbt"        ,  3    ,  0    , 26, 27, 0x00000001, 24, 24, 0x00000000, 20, 22, 0x00000007},
-        {"ldrt" ,  3    ,  0    , 26, 27, 0x00000001, 24, 24, 0x00000000, 20, 22, 0x00000003},
-        {"mrc"  ,  3    ,  6    , 24, 27, 0x0000000e, 20, 20, 0x00000001,  4,  4, 0x00000001},
-        {"mcr"  ,  3    ,  0    , 24, 27, 0x0000000e, 20, 20, 0x00000000,  4,  4, 0x00000001},
-        {"msr"  ,  2    ,  0    , 23, 27, 0x00000006, 20, 21, 0x00000002},
-        {"ldrb" ,  3    ,  0    , 26, 27, 0x00000001, 22, 22, 0x00000001, 20, 20, 0x00000001},
-        {"strb" ,  3    ,  0    , 26, 27, 0x00000001, 22, 22, 0x00000001, 20, 20, 0x00000000},
-        {"ldr"  ,  4    ,  0    , 28, 31, 0x0000000e, 26, 27, 0x00000001, 22, 22, 0x00000000, 20, 20, 0x00000001},
-        {"ldrcond"      ,  3    ,  0    , 26, 27, 0x00000001, 22, 22, 0x00000000, 20, 20, 0x00000001},
-        {"str"  ,  3    ,  0    , 26, 27, 0x00000001, 22, 22, 0x00000000, 20, 20, 0x00000000},
-        {"cdp"  ,  2    ,  0    , 24, 27, 0x0000000e,  4,  4, 0x00000000},
-        {"stc"  ,  2    ,  0    , 25, 27, 0x00000006, 20, 20, 0x00000000},
-        {"ldc"  ,  2    ,  0    , 25, 27, 0x00000006, 20, 20, 0x00000001},
-        {"swi"  ,  1    ,  0    , 24, 27, 0x0000000f},
-        {"bbl"  ,  1    ,  0    , 25, 27, 0x00000005},
+    { "vmla", 4, ARMVFP2, 23, 27, 0x1C, 20, 21, 0x0, 9, 11, 0x5, 4, 4, 0 },
+    { "vmls", 7, ARMVFP2, 28, 31, 0xF, 25, 27, 0x1, 23, 23, 1, 11, 11, 0, 8, 9, 0x2, 6, 6, 1, 4, 4, 0 },
+    { "vnmla", 4, ARMVFP2, 23, 27, 0x1C, 20, 21, 0x1, 9, 11, 0x5, 4, 4, 0 },
+    { "vnmla", 5, ARMVFP2, 23, 27, 0x1C, 20, 21, 0x2, 9, 11, 0x5, 6, 6, 1, 4, 4, 0 },
+    { "vnmls", 5, ARMVFP2, 23, 27, 0x1C, 20, 21, 0x1, 9, 11, 0x5, 6, 6, 0, 4, 4, 0 },
+    { "vnmul", 5, ARMVFP2, 23, 27, 0x1C, 20, 21, 0x2, 9, 11, 0x5, 6, 6, 1, 4, 4, 0 },
+    { "vmul", 5, ARMVFP2, 23, 27, 0x1C, 20, 21, 0x2, 9, 11, 0x5, 6, 6, 0, 4, 4, 0 },
+    { "vadd", 5, ARMVFP2, 23, 27, 0x1C, 20, 21, 0x3, 9, 11, 0x5, 6, 6, 0, 4, 4, 0 },
+    { "vsub", 5, ARMVFP2, 23, 27, 0x1C, 20, 21, 0x3, 9, 11, 0x5, 6, 6, 1, 4, 4, 0 },
+    { "vdiv", 5, ARMVFP2, 23, 27, 0x1D, 20, 21, 0x0, 9, 11, 0x5, 6, 6, 0, 4, 4, 0 },
+    { "vmov(i)", 4, ARMVFP3, 23, 27, 0x1D, 20, 21, 0x3, 9, 11, 0x5, 4, 7, 0 },
+    { "vmov(r)", 5, ARMVFP3, 23, 27, 0x1D, 16, 21, 0x30, 9, 11, 0x5, 6, 7, 1, 4, 4, 0 },
+    { "vabs", 5, ARMVFP2, 23, 27, 0x1D, 16, 21, 0x30, 9, 11, 0x5, 6, 7, 3, 4, 4, 0 },
+    { "vneg", 5, ARMVFP2, 23, 27, 0x1D, 17, 21, 0x18, 9, 11, 0x5, 6, 7, 1, 4, 4, 0 },
+    { "vsqrt", 5, ARMVFP2, 23, 27, 0x1D, 16, 21, 0x31, 9, 11, 0x5, 6, 7, 3, 4, 4, 0 },
+    { "vcmp", 5, ARMVFP2, 23, 27, 0x1D, 16, 21, 0x34, 9, 11, 0x5, 6, 6, 1, 4, 4, 0 },
+    { "vcmp2", 5, ARMVFP2, 23, 27, 0x1D, 16, 21, 0x35, 9, 11, 0x5, 0, 6, 0x40 },
+    { "vcvt(bds)", 5, ARMVFP2, 23, 27, 0x1D, 16, 21, 0x37, 9, 11, 0x5, 6, 7, 3, 4, 4, 0 },
+    { "vcvt(bff)", 6, ARMVFP3, 23, 27, 0x1D, 19, 21, 0x7, 17, 17, 0x1, 9, 11, 5, 6, 6, 1 },
+    { "vcvt(bfi)", 5, ARMVFP2, 23, 27, 0x1D, 19, 21, 0x7, 9, 11, 0x5, 6, 6, 1, 4, 4, 0 },
+    { "vmovbrs", 3, ARMVFP2, 21, 27, 0x70, 8, 11, 0xA, 0, 6, 0x10 },
+    { "vmsr", 2, ARMVFP2, 20, 27, 0xEE, 0, 11, 0xA10 },
+    { "vmovbrc", 4, ARMVFP2, 23, 27, 0x1C, 20, 20, 0x0, 8, 11, 0xB, 0, 4, 0x10 },
+    { "vmrs", 2, ARMVFP2, 20, 27, 0xEF, 0, 11, 0xA10 },
+    { "vmovbcr", 4, ARMVFP2, 24, 27, 0xE, 20, 20, 1, 8, 11, 0xB, 0, 4, 0x10 },
+    { "vmovbrrss", 3, ARMVFP2, 21, 27, 0x62, 8, 11, 0xA, 4, 4, 1 },
+    { "vmovbrrd", 3, ARMVFP2, 21, 27, 0x62, 6, 11, 0x2C, 4, 4, 1 },
+    { "vstr", 3, ARMVFP2, 24, 27, 0xD, 20, 21, 0, 9, 11, 5 },
+    { "vpush", 3, ARMVFP2, 23, 27, 0x1A, 16, 21, 0x2D, 9, 11, 5 },
+    { "vstm", 3, ARMVFP2, 25, 27, 0x6, 20, 20, 0, 9, 11, 5 },
+    { "vpop", 3, ARMVFP2, 23, 27, 0x19, 16, 21, 0x3D, 9, 11, 5 },
+    { "vldr", 3, ARMVFP2, 24, 27, 0xD, 20, 21, 1, 9, 11, 5 },
+    { "vldm", 3, ARMVFP2, 25, 27, 0x6, 20, 20, 1, 9, 11, 5 },
+
+    { "srs", 4, 6, 25, 31, 0x0000007c, 22, 22, 0x00000001, 16, 20, 0x0000000d, 8, 11, 0x00000005 },
+    { "rfe", 4, 6, 25, 31, 0x0000007c, 22, 22, 0x00000000, 20, 20, 0x00000001, 8, 11, 0x0000000a },
+    { "bkpt", 2, 3, 20, 31, 0x00000e12, 4, 7, 0x00000007 },
+    { "blx", 1, 3, 25, 31, 0x0000007d },
+    { "cps", 3, 6, 20, 31, 0x00000f10, 16, 16, 0x00000000, 5, 5, 0x00000000 },
+    { "pld", 4, 4, 26, 31, 0x0000003d, 24, 24, 0x00000001, 20, 22, 0x00000005, 12, 15, 0x0000000f },
+    { "setend", 2, 6, 16, 31, 0x0000f101, 4, 7, 0x00000000 },
+    { "clrex", 1, 6, 0, 31, 0xf57ff01f },
+    { "rev16", 2, 6, 16, 27, 0x000006bf, 4, 11, 0x000000fb },
+    { "usad8", 3, 6, 20, 27, 0x00000078, 12, 15, 0x0000000f, 4, 7, 0x00000001 },
+    { "sxtb", 2, 6, 16, 27, 0x000006af, 4, 7, 0x00000007 },
+    { "uxtb", 2, 6, 16, 27, 0x000006ef, 4, 7, 0x00000007 },
+    { "sxth", 2, 6, 16, 27, 0x000006bf, 4, 7, 0x00000007 },
+    { "sxtb16", 2, 6, 16, 27, 0x0000068f, 4, 7, 0x00000007 },
+    { "uxth", 2, 6, 16, 27, 0x000006ff, 4, 7, 0x00000007 },
+    { "uxtb16", 2, 6, 16, 27, 0x000006cf, 4, 7, 0x00000007 },
+    { "cpy", 2, 6, 20, 27, 0x0000001a, 4, 11, 0x00000000 },
+    { "uxtab", 2, 6, 20, 27, 0x0000006e, 4, 9, 0x00000007 },
+    { "ssub8", 2, 6, 20, 27, 0x00000061, 4, 7, 0x0000000f },
+    { "shsub8", 2, 6, 20, 27, 0x00000063, 4, 7, 0x0000000f },
+    { "ssubaddx", 2, 6, 20, 27, 0x00000061, 4, 7, 0x00000005 },
+    { "strex", 2, 6, 20, 27, 0x00000018, 4, 7, 0x00000009 },
+    { "strexb", 2, 7, 20, 27, 0x0000001c, 4, 7, 0x00000009 },
+    { "swp", 2, 0, 20, 27, 0x00000010, 4, 7, 0x00000009 },
+    { "swpb", 2, 0, 20, 27, 0x00000014, 4, 7, 0x00000009 },
+    { "ssub16", 2, 6, 20, 27, 0x00000061, 4, 7, 0x00000007 },
+    { "ssat16", 2, 6, 20, 27, 0x0000006a, 4, 7, 0x00000003 },
+    { "shsubaddx", 2, 6, 20, 27, 0x00000063, 4, 7, 0x00000005 },
+    { "qsubaddx", 2, 6, 20, 27, 0x00000062, 4, 7, 0x00000005 },
+    { "shaddsubx", 2, 6, 20, 27, 0x00000063, 4, 7, 0x00000003 },
+    { "shadd8", 2, 6, 20, 27, 0x00000063, 4, 7, 0x00000009 },
+    { "shadd16", 2, 6, 20, 27, 0x00000063, 4, 7, 0x00000001 },
+    { "sel", 2, 6, 20, 27, 0x00000068, 4, 7, 0x0000000b },
+    { "saddsubx", 2, 6, 20, 27, 0x00000061, 4, 7, 0x00000003 },
+    { "sadd8", 2, 6, 20, 27, 0x00000061, 4, 7, 0x00000009 },
+    { "sadd16", 2, 6, 20, 27, 0x00000061, 4, 7, 0x00000001 },
+    { "shsub16", 2, 6, 20, 27, 0x00000063, 4, 7, 0x00000007 },
+    { "umaal", 2, 6, 20, 27, 0x00000004, 4, 7, 0x00000009 },
+    { "uxtab16", 2, 6, 20, 27, 0x0000006c, 4, 7, 0x00000007 },
+    { "usubaddx", 2, 6, 20, 27, 0x00000065, 4, 7, 0x00000005 },
+    { "usub8", 2, 6, 20, 27, 0x00000065, 4, 7, 0x0000000f },
+    { "usub16", 2, 6, 20, 27, 0x00000065, 4, 7, 0x00000007 },
+    { "usat16", 2, 6, 20, 27, 0x0000006e, 4, 7, 0x00000003 },
+    { "usada8", 2, 6, 20, 27, 0x00000078, 4, 7, 0x00000001 },
+    { "uqsubaddx", 2, 6, 20, 27, 0x00000066, 4, 7, 0x00000005 },
+    { "uqsub8", 2, 6, 20, 27, 0x00000066, 4, 7, 0x0000000f },
+    { "uqsub16", 2, 6, 20, 27, 0x00000066, 4, 7, 0x00000007 },
+    { "uqaddsubx", 2, 6, 20, 27, 0x00000066, 4, 7, 0x00000003 },
+    { "uqadd8", 2, 6, 20, 27, 0x00000066, 4, 7, 0x00000009 },
+    { "uqadd16", 2, 6, 20, 27, 0x00000066, 4, 7, 0x00000001 },
+    { "sxtab", 2, 6, 20, 27, 0x0000006a, 4, 7, 0x00000007 },
+    { "uhsubaddx", 2, 6, 20, 27, 0x00000067, 4, 7, 0x00000005 },
+    { "uhsub8", 2, 6, 20, 27, 0x00000067, 4, 7, 0x0000000f },
+    { "uhsub16", 2, 6, 20, 27, 0x00000067, 4, 7, 0x00000007 },
+    { "uhaddsubx", 2, 6, 20, 27, 0x00000067, 4, 7, 0x00000003 },
+    { "uhadd8", 2, 6, 20, 27, 0x00000067, 4, 7, 0x00000009 },
+    { "uhadd16", 2, 6, 20, 27, 0x00000067, 4, 7, 0x00000001 },
+    { "uaddsubx", 2, 6, 20, 27, 0x00000065, 4, 7, 0x00000003 },
+    { "uadd8", 2, 6, 20, 27, 0x00000065, 4, 7, 0x00000009 },
+    { "uadd16", 2, 6, 20, 27, 0x00000065, 4, 7, 0x00000001 },
+    { "sxtah", 2, 6, 20, 27, 0x0000006b, 4, 7, 0x00000007 },
+    { "sxtab16", 2, 6, 20, 27, 0x00000068, 4, 7, 0x00000007 },
+    { "qadd8", 2, 6, 20, 27, 0x00000062, 4, 7, 0x00000009 },
+    { "bxj", 2, 5, 20, 27, 0x00000012, 4, 7, 0x00000002 },
+    { "clz", 2, 3, 20, 27, 0x00000016, 4, 7, 0x00000001 },
+    { "uxtah", 2, 6, 20, 27, 0x0000006f, 4, 7, 0x00000007 },
+    { "bx", 2, 2, 20, 27, 0x00000012, 4, 7, 0x00000001 },
+    { "rev", 2, 6, 20, 27, 0x0000006b, 4, 7, 0x00000003 },
+    { "blx", 2, 3, 20, 27, 0x00000012, 4, 7, 0x00000003 },
+    { "revsh", 2, 6, 20, 27, 0x0000006f, 4, 7, 0x0000000b },
+    { "qadd", 2, 4, 20, 27, 0x00000010, 4, 7, 0x00000005 },
+    { "qadd16", 2, 6, 20, 27, 0x00000062, 4, 7, 0x00000001 },
+    { "qaddsubx", 2, 6, 20, 27, 0x00000062, 4, 7, 0x00000003 },
+    { "ldrex", 2, 0, 20, 27, 0x00000019, 4, 7, 0x00000009 },
+    { "qdadd", 2, 4, 20, 27, 0x00000014, 4, 7, 0x00000005 },
+    { "qdsub", 2, 4, 20, 27, 0x00000016, 4, 7, 0x00000005 },
+    { "qsub", 2, 4, 20, 27, 0x00000012, 4, 7, 0x00000005 },
+    { "ldrexb", 2, 7, 20, 27, 0x0000001d, 4, 7, 0x00000009 },
+    { "qsub8", 2, 6, 20, 27, 0x00000062, 4, 7, 0x0000000f },
+    { "qsub16", 2, 6, 20, 27, 0x00000062, 4, 7, 0x00000007 },
+    { "smuad", 4, 6, 20, 27, 0x00000070, 12, 15, 0x0000000f, 6, 7, 0x00000000, 4, 4, 0x00000001 },
+    { "smmul", 4, 6, 20, 27, 0x00000075, 12, 15, 0x0000000f, 6, 7, 0x00000000, 4, 4, 0x00000001 },
+    { "smusd", 4, 6, 20, 27, 0x00000070, 12, 15, 0x0000000f, 6, 7, 0x00000001, 4, 4, 0x00000001 },
+    { "smlsd", 3, 6, 20, 27, 0x00000070, 6, 7, 0x00000001, 4, 4, 0x00000001 },
+    { "smlsld", 3, 6, 20, 27, 0x00000074, 6, 7, 0x00000001, 4, 4, 0x00000001 },
+    { "smmla", 3, 6, 20, 27, 0x00000075, 6, 7, 0x00000000, 4, 4, 0x00000001 },
+    { "smmls", 3, 6, 20, 27, 0x00000075, 6, 7, 0x00000003, 4, 4, 0x00000001 },
+    { "smlald", 3, 6, 20, 27, 0x00000074, 6, 7, 0x00000000, 4, 4, 0x00000001 },
+    { "smlad", 3, 6, 20, 27, 0x00000070, 6, 7, 0x00000000, 4, 4, 0x00000001 },
+    { "smlaw", 3, 4, 20, 27, 0x00000012, 7, 7, 0x00000001, 4, 5, 0x00000000 },
+    { "smulw", 3, 4, 20, 27, 0x00000012, 7, 7, 0x00000001, 4, 5, 0x00000002 },
+    { "pkhtb", 2, 6, 20, 27, 0x00000068, 4, 6, 0x00000005 },
+    { "pkhbt", 2, 6, 20, 27, 0x00000068, 4, 6, 0x00000001 },
+    { "smul", 3, 4, 20, 27, 0x00000016, 7, 7, 0x00000001, 4, 4, 0x00000000 },
+    { "smlalxy", 3, 4, 20, 27, 0x00000014, 7, 7, 0x00000001, 4, 4, 0x00000000 },
+    //  {"smlal"        ,  2    ,  4    , 21, 27, 0x00000007,  4,  7, 0x00000009},
+    { "smla", 3, 4, 20, 27, 0x00000010, 7, 7, 0x00000001, 4, 4, 0x00000000 },
+    { "mcrr", 1, 6, 20, 27, 0x000000c4 },
+    { "mrrc", 1, 6, 20, 27, 0x000000c5 },
+    { "cmp", 2, 0, 26, 27, 0x00000000, 20, 24, 0x00000015 },
+    { "tst", 2, 0, 26, 27, 0x00000000, 20, 24, 0x00000011 },
+    { "teq", 2, 0, 26, 27, 0x00000000, 20, 24, 0x00000013 },
+    { "cmn", 2, 0, 26, 27, 0x00000000, 20, 24, 0x00000017 },
+    { "smull", 2, 0, 21, 27, 0x00000006, 4, 7, 0x00000009 },
+    { "umull", 2, 0, 21, 27, 0x00000004, 4, 7, 0x00000009 },
+    { "umlal", 2, 0, 21, 27, 0x00000005, 4, 7, 0x00000009 },
+    { "smlal", 2, 0, 21, 27, 0x00000007, 4, 7, 0x00000009 },
+    { "mul", 2, 0, 21, 27, 0x00000000, 4, 7, 0x00000009 },
+    { "mla", 2, 0, 21, 27, 0x00000001, 4, 7, 0x00000009 },
+    { "ssat", 2, 6, 21, 27, 0x00000035, 4, 5, 0x00000001 },
+    { "usat", 2, 6, 21, 27, 0x00000037, 4, 5, 0x00000001 },
+    { "mrs", 4, 0, 23, 27, 0x00000002, 20, 21, 0x00000000, 16, 19, 0x0000000f, 0, 11, 0x00000000 },
+    { "msr", 3, 0, 23, 27, 0x00000002, 20, 21, 0x00000002, 4, 7, 0x00000000 },
+    { "and", 2, 0, 26, 27, 0x00000000, 21, 24, 0x00000000 },
+    { "bic", 2, 0, 26, 27, 0x00000000, 21, 24, 0x0000000e },
+    { "ldm", 3, 0, 25, 27, 0x00000004, 20, 22, 0x00000005, 15, 15, 0x00000000 },
+    { "eor", 2, 0, 26, 27, 0x00000000, 21, 24, 0x00000001 },
+    { "add", 2, 0, 26, 27, 0x00000000, 21, 24, 0x00000004 },
+    { "rsb", 2, 0, 26, 27, 0x00000000, 21, 24, 0x00000003 },
+    { "rsc", 2, 0, 26, 27, 0x00000000, 21, 24, 0x00000007 },
+    { "sbc", 2, 0, 26, 27, 0x00000000, 21, 24, 0x00000006 },
+    { "adc", 2, 0, 26, 27, 0x00000000, 21, 24, 0x00000005 },
+    { "sub", 2, 0, 26, 27, 0x00000000, 21, 24, 0x00000002 },
+    { "orr", 2, 0, 26, 27, 0x00000000, 21, 24, 0x0000000c },
+    { "mvn", 2, 0, 26, 27, 0x00000000, 21, 24, 0x0000000f },
+    { "mov", 2, 0, 26, 27, 0x00000000, 21, 24, 0x0000000d },
+    { "stm", 2, 0, 25, 27, 0x00000004, 20, 22, 0x00000004 },
+    { "ldm", 4, 0, 25, 27, 0x00000004, 22, 22, 0x00000001, 20, 20, 0x00000001, 15, 15, 0x00000001 },
+    { "ldrsh", 3, 2, 25, 27, 0x00000000, 20, 20, 0x00000001, 4, 7, 0x0000000f },
+    { "stm", 3, 0, 25, 27, 0x00000004, 22, 22, 0x00000000, 20, 20, 0x00000000 },
+    { "ldm", 3, 0, 25, 27, 0x00000004, 22, 22, 0x00000000, 20, 20, 0x00000001 },
+    { "ldrsb", 3, 2, 25, 27, 0x00000000, 20, 20, 0x00000001, 4, 7, 0x0000000d },
+    { "strd", 3, 4, 25, 27, 0x00000000, 20, 20, 0x00000000, 4, 7, 0x0000000f },
+    { "ldrh", 3, 0, 25, 27, 0x00000000, 20, 20, 0x00000001, 4, 7, 0x0000000b },
+    { "strh", 3, 0, 25, 27, 0x00000000, 20, 20, 0x00000000, 4, 7, 0x0000000b },
+    { "ldrd", 3, 4, 25, 27, 0x00000000, 20, 20, 0x00000000, 4, 7, 0x0000000d },
+    { "strt", 3, 0, 26, 27, 0x00000001, 24, 24, 0x00000000, 20, 22, 0x00000002 },
+    { "strbt", 3, 0, 26, 27, 0x00000001, 24, 24, 0x00000000, 20, 22, 0x00000006 },
+    { "ldrbt", 3, 0, 26, 27, 0x00000001, 24, 24, 0x00000000, 20, 22, 0x00000007 },
+    { "ldrt", 3, 0, 26, 27, 0x00000001, 24, 24, 0x00000000, 20, 22, 0x00000003 },
+    { "mrc", 3, 6, 24, 27, 0x0000000e, 20, 20, 0x00000001, 4, 4, 0x00000001 },
+    { "mcr", 3, 0, 24, 27, 0x0000000e, 20, 20, 0x00000000, 4, 4, 0x00000001 },
+    { "msr", 2, 0, 23, 27, 0x00000006, 20, 21, 0x00000002 },
+    { "ldrb", 3, 0, 26, 27, 0x00000001, 22, 22, 0x00000001, 20, 20, 0x00000001 },
+    { "strb", 3, 0, 26, 27, 0x00000001, 22, 22, 0x00000001, 20, 20, 0x00000000 },
+    { "ldr", 4, 0, 28, 31, 0x0000000e, 26, 27, 0x00000001, 22, 22, 0x00000000, 20, 20, 0x00000001 },
+    { "ldrcond", 3, 0, 26, 27, 0x00000001, 22, 22, 0x00000000, 20, 20, 0x00000001 },
+    { "str", 3, 0, 26, 27, 0x00000001, 22, 22, 0x00000000, 20, 20, 0x00000000 },
+    { "cdp", 2, 0, 24, 27, 0x0000000e, 4, 4, 0x00000000 },
+    { "stc", 2, 0, 25, 27, 0x00000006, 20, 20, 0x00000000 },
+    { "ldc", 2, 0, 25, 27, 0x00000006, 20, 20, 0x00000001 },
+    { "swi", 1, 0, 24, 27, 0x0000000f },
+    { "bbl", 1, 0, 25, 27, 0x00000005 },
 };
 
 const ISEITEM arm_exclusion_code[] = {
-        #define VFP_DECODE_EXCLUSION
-        #include "core/arm/skyeye_common/vfp/vfpinstr.cpp"
-        #undef VFP_DECODE_EXCLUSION
-        {"srs"  ,  0    ,  6    ,  0},
-        {"rfe"  ,  0    ,  6    ,  0},
-        {"bkpt" ,  0    ,  3    ,  0},
-        {"blx"  ,  0    ,  3    ,  0},
-        {"cps"  ,  0    ,  6    ,  0},
-        {"pld"  ,  0    ,  4    ,  0},
-        {"setend"       ,  0    ,  6    ,  0},
-        {"clrex"        ,  0    ,  6    ,  0},
-        {"rev16"        ,  0    ,  6    ,  0},
-        {"usad8"        ,  0    ,  6    ,  0},
-        {"sxtb" ,  0    ,  6    ,  0},
-        {"uxtb" ,  0    ,  6    ,  0},
-        {"sxth" ,  0    ,  6    ,  0},
-        {"sxtb16"       ,  0    ,  6    ,  0},
-        {"uxth" ,  0    ,  6    ,  0},
-        {"uxtb16"       ,  0    ,  6    ,  0},
-        {"cpy"  ,  0    ,  6    ,  0},
-        {"uxtab"        ,  0    ,  6    ,  0},
-        {"ssub8"        ,  0    ,  6    ,  0},
-        {"shsub8"       ,  0    ,  6    ,  0},
-        {"ssubaddx"     ,  0    ,  6    ,  0},
-        {"strex"        ,  0    ,  6    ,  0},
-        {"strexb"       ,  0    ,  7    ,  0},
-        {"swp"  ,  0    ,  0    ,  0},
-        {"swpb" ,  0    ,  0    ,  0},
-        {"ssub16"       ,  0    ,  6    ,  0},
-        {"ssat16"       ,  0    ,  6    ,  0},
-        {"shsubaddx"    ,  0    ,  6    ,  0},
-        {"qsubaddx"     ,  0    ,  6    ,  0},
-        {"shaddsubx"    ,  0    ,  6    ,  0},
-        {"shadd8"       ,  0    ,  6    ,  0},
-        {"shadd16"      ,  0    ,  6    ,  0},
-        {"sel"  ,  0    ,  6    ,  0},
-        {"saddsubx"     ,  0    ,  6    ,  0},
-        {"sadd8"        ,  0    ,  6    ,  0},
-        {"sadd16"       ,  0    ,  6    ,  0},
-        {"shsub16"      ,  0    ,  6    ,  0},
-        {"umaal"        ,  0    ,  6    ,  0},
-        {"uxtab16"      ,  0    ,  6    ,  0},
-        {"usubaddx"     ,  0    ,  6    ,  0},
-        {"usub8"        ,  0    ,  6    ,  0},
-        {"usub16"       ,  0    ,  6    ,  0},
-        {"usat16"       ,  0    ,  6    ,  0},
-        {"usada8"       ,  0    ,  6    ,  0},
-        {"uqsubaddx"    ,  0    ,  6    ,  0},
-        {"uqsub8"       ,  0    ,  6    ,  0},
-        {"uqsub16"      ,  0    ,  6    ,  0},
-        {"uqaddsubx"    ,  0    ,  6    ,  0},
-        {"uqadd8"       ,  0    ,  6    ,  0},
-        {"uqadd16"      ,  0    ,  6    ,  0},
-        {"sxtab"        ,  0    ,  6    ,  0},
-        {"uhsubaddx"    ,  0    ,  6    ,  0},
-        {"uhsub8"       ,  0    ,  6    ,  0},
-        {"uhsub16"      ,  0    ,  6    ,  0},
-        {"uhaddsubx"    ,  0    ,  6    ,  0},
-        {"uhadd8"       ,  0    ,  6    ,  0},
-        {"uhadd16"      ,  0    ,  6    ,  0},
-        {"uaddsubx"     ,  0    ,  6    ,  0},
-        {"uadd8"        ,  0    ,  6    ,  0},
-        {"uadd16"       ,  0    ,  6    ,  0},
-        {"sxtah"        ,  0    ,  6    ,  0},
-        {"sxtab16"      ,  0    ,  6    ,  0},
-        {"qadd8"        ,  0    ,  6    ,  0},
-        {"bxj"  ,  0    ,  5    ,  0},
-        {"clz"  ,  0    ,  3    ,  0},
-        {"uxtah"        ,  0    ,  6    ,  0},
-        {"bx"   ,  0    ,  2    ,  0},
-        {"rev"  ,  0    ,  6    ,  0},
-        {"blx"  ,  0    ,  3    ,  0},
-        {"revsh"        ,  0    ,  6    ,  0},
-        {"qadd" ,  0    ,  4    ,  0},
-        {"qadd16"       ,  0    ,  6    ,  0},
-        {"qaddsubx"     ,  0    ,  6    ,  0},
-        {"ldrex"        ,  0    ,  0    ,  0},
-        {"qdadd"        ,  0    ,  4    ,  0},
-        {"qdsub"        ,  0    ,  4    ,  0},
-        {"qsub" ,  0    ,  4    ,  0},
-        {"ldrexb"       ,  0    ,  7    ,  0},
-        {"qsub8"        ,  0    ,  6    ,  0},
-        {"qsub16"       ,  0    ,  6    ,  0},
-        {"smuad"        ,  0    ,  6    ,  0},
-        {"smmul"        ,  0    ,  6    ,  0},
-        {"smusd"        ,  0    ,  6    ,  0},
-        {"smlsd"        ,  0    ,  6    ,  0},
-        {"smlsld"       ,  0    ,  6    ,  0},
-        {"smmla"        ,  0    ,  6    ,  0},
-        {"smmls"        ,  0    ,  6    ,  0},
-        {"smlald"       ,  0    ,  6    ,  0},
-        {"smlad"        ,  0    ,  6    ,  0},
-        {"smlaw"        ,  0    ,  4    ,  0},
-        {"smulw"        ,  0    ,  4    ,  0},
-        {"pkhtb"        ,  0    ,  6    ,  0},
-        {"pkhbt"        ,  0    ,  6    ,  0},
-        {"smul" ,  0    ,  4    ,  0},
-        {"smlal"        ,  0    ,  4    ,  0},
-        {"smla" ,  0    ,  4    ,  0},
-        {"mcrr" ,  0    ,  6    ,  0},
-        {"mrrc" ,  0    ,  6    ,  0},
-        {"cmp"  ,  3    ,  0    ,  4,  4, 0x00000001,  7,  7, 0x00000001, 25, 25, 0x00000000},
-        {"tst"  ,  3    ,  0    ,  4,  4, 0x00000001,  7,  7, 0x00000001, 25, 25, 0x00000000},
-        {"teq"  ,  3    ,  0    ,  4,  4, 0x00000001,  7,  7, 0x00000001, 25, 25, 0x00000000},
-        {"cmn"  ,  3    ,  0    ,  4,  4, 0x00000001,  7,  7, 0x00000001, 25, 25, 0x00000000},
-        {"smull"        ,  0    ,  0    ,  0},
-        {"umull"        ,  0    ,  0    ,  0},
-        {"umlal"        ,  0    ,  0    ,  0},
-        {"smlal"        ,  0    ,  0    ,  0},
-        {"mul"  ,  0    ,  0    ,  0},
-        {"mla"  ,  0    ,  0    ,  0},
-        {"ssat" ,  0    ,  6    ,  0},
-        {"usat" ,  0    ,  6    ,  0},
-        {"mrs"  ,  0    ,  0    ,  0},
-        {"msr"  ,  0    ,  0    ,  0},
-        {"and"  ,  3    ,  0    ,  4,  4, 0x00000001,  7,  7, 0x00000001, 25, 25, 0x00000000},
-        {"bic"  ,  3    ,  0    ,  4,  4, 0x00000001,  7,  7, 0x00000001, 25, 25, 0x00000000},
-        {"ldm"  ,  0    ,  0    ,  0},
-        {"eor"  ,  3    ,  0    ,  4,  4, 0x00000001,  7,  7, 0x00000001, 25, 25, 0x00000000},
-        {"add"  ,  3    ,  0    ,  4,  4, 0x00000001,  7,  7, 0x00000001, 25, 25, 0x00000000},
-        {"rsb"  ,  3    ,  0    ,  4,  4, 0x00000001,  7,  7, 0x00000001, 25, 25, 0x00000000},
-        {"rsc"  ,  3    ,  0    ,  4,  4, 0x00000001,  7,  7, 0x00000001, 25, 25, 0x00000000},
-        {"sbc"  ,  3    ,  0    ,  4,  4, 0x00000001,  7,  7, 0x00000001, 25, 25, 0x00000000},
-        {"adc"  ,  3    ,  0    ,  4,  4, 0x00000001,  7,  7, 0x00000001, 25, 25, 0x00000000},
-        {"sub"  ,  3    ,  0    ,  4,  4, 0x00000001,  7,  7, 0x00000001, 25, 25, 0x00000000},
-        {"orr"  ,  3    ,  0    ,  4,  4, 0x00000001,  7,  7, 0x00000001, 25, 25, 0x00000000},
-        {"mvn"  ,  3    ,  0    ,  4,  4, 0x00000001,  7,  7, 0x00000001, 25, 25, 0x00000000},
-        {"mov"  ,  3    ,  0    ,  4,  4, 0x00000001,  7,  7, 0x00000001, 25, 25, 0x00000000},
-        {"stm"  ,  0    ,  0    ,  0},
-        {"ldm"  ,  0    ,  0    ,  0},
-        {"ldrsh"        ,  0    ,  2    ,  0},
-        {"stm"  ,  0    ,  0    ,  0},
-        {"ldm"  ,  0    ,  0    ,  0},
-        {"ldrsb"        ,  0    ,  2    ,  0},
-        {"strd" ,  0    ,  4    ,  0},
-        {"ldrh" ,  0    ,  0    ,  0},
-        {"strh" ,  0    ,  0    ,  0},
-        {"ldrd" ,  0    ,  4    ,  0},
-        {"strt" ,  0    ,  0    ,  0},
-        {"strbt"        ,  0    ,  0    ,  0},
-        {"ldrbt"        ,  0    ,  0    ,  0},
-        {"ldrt" ,  0    ,  0    ,  0},
-        {"mrc"  ,  0    ,  6    ,  0},
-        {"mcr"  ,  0    ,  0    ,  0},
-        {"msr"  ,  0    ,  0    ,  0},
-        {"ldrb" ,  0    ,  0    ,  0},
-        {"strb" ,  0    ,  0    ,  0},
-        {"ldr"  ,  0    ,  0    ,  0},
-        {"ldrcond"      ,  1    ,  0    ,  28, 31, 0x0000000e},
-        {"str"  ,  0    ,  0    ,  0},
-        {"cdp"  ,  0    ,  0    ,  0},
-        {"stc"  ,  0    ,  0    ,  0},
-        {"ldc"  ,  0    ,  0    ,  0},
-        {"swi"  ,  0    ,  0    ,  0},
-        {"bbl"  ,  0    ,  0    ,  0},
-        {"bl_1_thumb",      0,      INVALID, 0},/* should be table[-4] */         
-        {"bl_2_thumb",      0,      INVALID, 0}, /* should be located at the end of the table[-3] */
-        {"blx_1_thumb",      0,      INVALID, 0}, /* should be located at table[-2] */
-        {"invalid",      0,      INVALID, 0}         
+    { "vmla", 0, ARMVFP2, 0 },
+    { "vmls", 0, ARMVFP2, 0 },
+    { "vnmla", 0, ARMVFP2, 0 },
+    { "vnmla", 0, ARMVFP2, 0 },
+    { "vnmls", 0, ARMVFP2, 0 },
+    { "vnmul", 0, ARMVFP2, 0 },
+    { "vmul", 0, ARMVFP2, 0 },
+    { "vadd", 0, ARMVFP2, 0 },
+    { "vsub", 0, ARMVFP2, 0 },
+    { "vdiv", 0, ARMVFP2, 0 },
+    { "vmov(i)", 0, ARMVFP3, 0 },
+    { "vmov(r)", 0, ARMVFP3, 0 },
+    { "vabs", 0, ARMVFP2, 0 },
+    { "vneg", 0, ARMVFP2, 0 },
+    { "vsqrt", 0, ARMVFP2, 0 },
+    { "vcmp", 0, ARMVFP2, 0 },
+    { "vcmp2", 0, ARMVFP2, 0 },
+    { "vcvt(bff)", 0, ARMVFP3, 4, 4, 1 },
+    { "vcvt(bds)", 0, ARMVFP2, 0 },
+    { "vcvt(bfi)", 0, ARMVFP2, 0 },
+    { "vmovbrs", 0, ARMVFP2, 0 },
+    { "vmsr", 0, ARMVFP2, 0 },
+    { "vmovbrc", 0, ARMVFP2, 0 },
+    { "vmrs", 0, ARMVFP2, 0 },
+    { "vmovbcr", 0, ARMVFP2, 0 },
+    { "vmovbrrss", 0, ARMVFP2, 0 },
+    { "vmovbrrd", 0, ARMVFP2, 0 },
+    { "vstr", 0, ARMVFP2, 0 },
+    { "vpush", 0, ARMVFP2, 0 },
+    { "vstm", 0, ARMVFP2, 0 },
+    { "vpop", 0, ARMVFP2, 0 },
+    { "vldr", 0, ARMVFP2, 0 },
+    { "vldm", 0, ARMVFP2, 0 },
+
+    { "srs", 0, 6, 0 },
+    { "rfe", 0, 6, 0 },
+    { "bkpt", 0, 3, 0 },
+    { "blx", 0, 3, 0 },
+    { "cps", 0, 6, 0 },
+    { "pld", 0, 4, 0 },
+    { "setend", 0, 6, 0 },
+    { "clrex", 0, 6, 0 },
+    { "rev16", 0, 6, 0 },
+    { "usad8", 0, 6, 0 },
+    { "sxtb", 0, 6, 0 },
+    { "uxtb", 0, 6, 0 },
+    { "sxth", 0, 6, 0 },
+    { "sxtb16", 0, 6, 0 },
+    { "uxth", 0, 6, 0 },
+    { "uxtb16", 0, 6, 0 },
+    { "cpy", 0, 6, 0 },
+    { "uxtab", 0, 6, 0 },
+    { "ssub8", 0, 6, 0 },
+    { "shsub8", 0, 6, 0 },
+    { "ssubaddx", 0, 6, 0 },
+    { "strex", 0, 6, 0 },
+    { "strexb", 0, 7, 0 },
+    { "swp", 0, 0, 0 },
+    { "swpb", 0, 0, 0 },
+    { "ssub16", 0, 6, 0 },
+    { "ssat16", 0, 6, 0 },
+    { "shsubaddx", 0, 6, 0 },
+    { "qsubaddx", 0, 6, 0 },
+    { "shaddsubx", 0, 6, 0 },
+    { "shadd8", 0, 6, 0 },
+    { "shadd16", 0, 6, 0 },
+    { "sel", 0, 6, 0 },
+    { "saddsubx", 0, 6, 0 },
+    { "sadd8", 0, 6, 0 },
+    { "sadd16", 0, 6, 0 },
+    { "shsub16", 0, 6, 0 },
+    { "umaal", 0, 6, 0 },
+    { "uxtab16", 0, 6, 0 },
+    { "usubaddx", 0, 6, 0 },
+    { "usub8", 0, 6, 0 },
+    { "usub16", 0, 6, 0 },
+    { "usat16", 0, 6, 0 },
+    { "usada8", 0, 6, 0 },
+    { "uqsubaddx", 0, 6, 0 },
+    { "uqsub8", 0, 6, 0 },
+    { "uqsub16", 0, 6, 0 },
+    { "uqaddsubx", 0, 6, 0 },
+    { "uqadd8", 0, 6, 0 },
+    { "uqadd16", 0, 6, 0 },
+    { "sxtab", 0, 6, 0 },
+    { "uhsubaddx", 0, 6, 0 },
+    { "uhsub8", 0, 6, 0 },
+    { "uhsub16", 0, 6, 0 },
+    { "uhaddsubx", 0, 6, 0 },
+    { "uhadd8", 0, 6, 0 },
+    { "uhadd16", 0, 6, 0 },
+    { "uaddsubx", 0, 6, 0 },
+    { "uadd8", 0, 6, 0 },
+    { "uadd16", 0, 6, 0 },
+    { "sxtah", 0, 6, 0 },
+    { "sxtab16", 0, 6, 0 },
+    { "qadd8", 0, 6, 0 },
+    { "bxj", 0, 5, 0 },
+    { "clz", 0, 3, 0 },
+    { "uxtah", 0, 6, 0 },
+    { "bx", 0, 2, 0 },
+    { "rev", 0, 6, 0 },
+    { "blx", 0, 3, 0 },
+    { "revsh", 0, 6, 0 },
+    { "qadd", 0, 4, 0 },
+    { "qadd16", 0, 6, 0 },
+    { "qaddsubx", 0, 6, 0 },
+    { "ldrex", 0, 0, 0 },
+    { "qdadd", 0, 4, 0 },
+    { "qdsub", 0, 4, 0 },
+    { "qsub", 0, 4, 0 },
+    { "ldrexb", 0, 7, 0 },
+    { "qsub8", 0, 6, 0 },
+    { "qsub16", 0, 6, 0 },
+    { "smuad", 0, 6, 0 },
+    { "smmul", 0, 6, 0 },
+    { "smusd", 0, 6, 0 },
+    { "smlsd", 0, 6, 0 },
+    { "smlsld", 0, 6, 0 },
+    { "smmla", 0, 6, 0 },
+    { "smmls", 0, 6, 0 },
+    { "smlald", 0, 6, 0 },
+    { "smlad", 0, 6, 0 },
+    { "smlaw", 0, 4, 0 },
+    { "smulw", 0, 4, 0 },
+    { "pkhtb", 0, 6, 0 },
+    { "pkhbt", 0, 6, 0 },
+    { "smul", 0, 4, 0 },
+    { "smlal", 0, 4, 0 },
+    { "smla", 0, 4, 0 },
+    { "mcrr", 0, 6, 0 },
+    { "mrrc", 0, 6, 0 },
+    { "cmp", 3, 0, 4, 4, 0x00000001, 7, 7, 0x00000001, 25, 25, 0x00000000 },
+    { "tst", 3, 0, 4, 4, 0x00000001, 7, 7, 0x00000001, 25, 25, 0x00000000 },
+    { "teq", 3, 0, 4, 4, 0x00000001, 7, 7, 0x00000001, 25, 25, 0x00000000 },
+    { "cmn", 3, 0, 4, 4, 0x00000001, 7, 7, 0x00000001, 25, 25, 0x00000000 },
+    { "smull", 0, 0, 0 },
+    { "umull", 0, 0, 0 },
+    { "umlal", 0, 0, 0 },
+    { "smlal", 0, 0, 0 },
+    { "mul", 0, 0, 0 },
+    { "mla", 0, 0, 0 },
+    { "ssat", 0, 6, 0 },
+    { "usat", 0, 6, 0 },
+    { "mrs", 0, 0, 0 },
+    { "msr", 0, 0, 0 },
+    { "and", 3, 0, 4, 4, 0x00000001, 7, 7, 0x00000001, 25, 25, 0x00000000 },
+    { "bic", 3, 0, 4, 4, 0x00000001, 7, 7, 0x00000001, 25, 25, 0x00000000 },
+    { "ldm", 0, 0, 0 },
+    { "eor", 3, 0, 4, 4, 0x00000001, 7, 7, 0x00000001, 25, 25, 0x00000000 },
+    { "add", 3, 0, 4, 4, 0x00000001, 7, 7, 0x00000001, 25, 25, 0x00000000 },
+    { "rsb", 3, 0, 4, 4, 0x00000001, 7, 7, 0x00000001, 25, 25, 0x00000000 },
+    { "rsc", 3, 0, 4, 4, 0x00000001, 7, 7, 0x00000001, 25, 25, 0x00000000 },
+    { "sbc", 3, 0, 4, 4, 0x00000001, 7, 7, 0x00000001, 25, 25, 0x00000000 },
+    { "adc", 3, 0, 4, 4, 0x00000001, 7, 7, 0x00000001, 25, 25, 0x00000000 },
+    { "sub", 3, 0, 4, 4, 0x00000001, 7, 7, 0x00000001, 25, 25, 0x00000000 },
+    { "orr", 3, 0, 4, 4, 0x00000001, 7, 7, 0x00000001, 25, 25, 0x00000000 },
+    { "mvn", 3, 0, 4, 4, 0x00000001, 7, 7, 0x00000001, 25, 25, 0x00000000 },
+    { "mov", 3, 0, 4, 4, 0x00000001, 7, 7, 0x00000001, 25, 25, 0x00000000 },
+    { "stm", 0, 0, 0 },
+    { "ldm", 0, 0, 0 },
+    { "ldrsh", 0, 2, 0 },
+    { "stm", 0, 0, 0 },
+    { "ldm", 0, 0, 0 },
+    { "ldrsb", 0, 2, 0 },
+    { "strd", 0, 4, 0 },
+    { "ldrh", 0, 0, 0 },
+    { "strh", 0, 0, 0 },
+    { "ldrd", 0, 4, 0 },
+    { "strt", 0, 0, 0 },
+    { "strbt", 0, 0, 0 },
+    { "ldrbt", 0, 0, 0 },
+    { "ldrt", 0, 0, 0 },
+    { "mrc", 0, 6, 0 },
+    { "mcr", 0, 0, 0 },
+    { "msr", 0, 0, 0 },
+    { "ldrb", 0, 0, 0 },
+    { "strb", 0, 0, 0 },
+    { "ldr", 0, 0, 0 },
+    { "ldrcond", 1, 0, 28, 31, 0x0000000e },
+    { "str", 0, 0, 0 },
+    { "cdp", 0, 0, 0 },
+    { "stc", 0, 0, 0 },
+    { "ldc", 0, 0, 0 },
+    { "swi", 0, 0, 0 },
+    { "bbl", 0, 0, 0 },
+    { "bl_1_thumb", 0, INVALID, 0 },    // Should be table[-4]
+    { "bl_2_thumb", 0, INVALID, 0 },    // Should be located at the end of the table[-3]
+    { "blx_1_thumb", 0, INVALID, 0 },   // Should be located at table[-2]
+    { "invalid", 0, INVALID, 0 }
 };
 
-int decode_arm_instr(uint32_t instr, int32_t *idx)
-{
-        int n = 0;
-        int base = 0;
-        int ret = DECODE_FAILURE;
-        int i = 0;
-        int instr_slots = sizeof(arm_instruction)/sizeof(ISEITEM);
-        for (i = 0; i < instr_slots; i++)
-        {
-//              ret = DECODE_SUCCESS;
-                n = arm_instruction[i].attribute_value;
-                base = 0;
-                while (n) {
-                        if (arm_instruction[i].content[base + 1] == 31 && arm_instruction[i].content[base] == 0) {
-                                /* clrex */
-                                if (instr != arm_instruction[i].content[base + 2]) {
-                                        break;
-                                }
-                        } else if (BITS(arm_instruction[i].content[base], arm_instruction[i].content[base + 1]) != arm_instruction[i].content[base + 2]) {
-                                break;
-                        }
-                        base += 3;
-                        n --;
-                }
-                //All conditions is satisfied.
-                if (n == 0)
-                        ret = DECODE_SUCCESS;
+int decode_arm_instr(uint32_t instr, int32_t *idx) {
+    int n = 0;
+    int base = 0;
+    int ret = DECODE_FAILURE;
+    int i = 0;
+    int instr_slots = sizeof(arm_instruction) / sizeof(ISEITEM);
+    for (i = 0; i < instr_slots; i++) {
+        n = arm_instruction[i].attribute_value;
+        base = 0;
 
-                if (ret == DECODE_SUCCESS) {
-                        n = arm_exclusion_code[i].attribute_value;
-                        if (n != 0) {
-                                base = 0;
-                                while (n) {
-                                        if (BITS(arm_exclusion_code[i].content[base], arm_exclusion_code[i].content[base + 1]) != arm_exclusion_code[i].content[base + 2]) {
-                                                break;                                  }
-                                        base += 3;
-                                        n --;
-                                }
-                                //All conditions is satisfied.
-                                if (n == 0)
-                                        ret = DECODE_FAILURE;
-                        }
-                }
+        while (n) {
+            if (arm_instruction[i].content[base + 1] == 31 && arm_instruction[i].content[base] == 0) {
+                // clrex
+                if (instr != arm_instruction[i].content[base + 2]) {
+                    break;
+                }
+            } else if (BITS(arm_instruction[i].content[base], arm_instruction[i].content[base + 1]) != arm_instruction[i].content[base + 2]) {
+                break;
+            }
+            base += 3;
+            n--;
+        }
 
-                if (ret == DECODE_SUCCESS) {
-                        *idx = i;
-                        return ret;
-                }
-        }
-        return ret;
-}
+        // All conditions is satisfied.
+        if (n == 0)
+            ret = DECODE_SUCCESS;
 
+        if (ret == DECODE_SUCCESS) {
+            n = arm_exclusion_code[i].attribute_value;
+            if (n != 0) {
+                base = 0;
+                while (n) {
+                    if (BITS(arm_exclusion_code[i].content[base], arm_exclusion_code[i].content[base + 1]) != arm_exclusion_code[i].content[base + 2]) {
+                        break;
+                    }
+                    base += 3;
+                    n--;
+                }
+
+                // All conditions is satisfied.
+                if (n == 0)
+                    ret = DECODE_FAILURE;
+            }
+        }
+
+        if (ret == DECODE_SUCCESS) {
+            *idx = i;
+            return ret;
+        }
+    }
+    return ret;
+}
diff --git a/src/core/arm/dyncom/arm_dyncom_dec.h b/src/core/arm/dyncom/arm_dyncom_dec.h
index 19d94f369..70eb96e93 100644
--- a/src/core/arm/dyncom/arm_dyncom_dec.h
+++ b/src/core/arm/dyncom/arm_dyncom_dec.h
@@ -56,8 +56,6 @@
 #define RN ((instr >> 16) & 0xF)
 /*xxxx xxxx xxxx xxxx xxxx xxxx xxxx 1111 */
 #define RM (instr & 0xF)
-#define BIT(n) ((instr >> (n)) & 1)
-#define BITS(a,b) ((instr >> (a)) & ((1 << (1+(b)-(a)))-1))
 
 /* CP15 registers */
 #define OPCODE_1        BITS(21, 23)
diff --git a/src/core/arm/dyncom/arm_dyncom_interpreter.cpp b/src/core/arm/dyncom/arm_dyncom_interpreter.cpp
index 68012bffd..0ee103c56 100644
--- a/src/core/arm/dyncom/arm_dyncom_interpreter.cpp
+++ b/src/core/arm/dyncom/arm_dyncom_interpreter.cpp
@@ -1,27 +1,6 @@
-/* Copyright (C) 
-* 2012 - Michael.Kang blackfin.kang@gmail.com
-* This program is free software; you can redistribute it and/or
-* modify it under the terms of the GNU General Public License
-* as published by the Free Software Foundation; either version 2
-* of the License, or (at your option) any later version.
-* 
-* This program is distributed in the hope that 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, write to the Free Software
-* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
-* 
-*/
-/**
-* @file arm_dyncom_interpreter.cpp
-* @brief The fast interpreter for arm
-* @author Michael.Kang blackfin.kang@gmail.com
-* @version 7849
-* @date 2012-03-15
-*/
+// Copyright 2012 Michael Kang, 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
 
 #define CITRA_IGNORE_EXIT(x)
 
@@ -39,10 +18,6 @@ using namespace std;
 #include "arm_dyncom_thumb.h"
 #include "arm_dyncom_run.h"
 #include "core/arm/skyeye_common/vfp/vfp.h"
-/* shenoubang 2012-6-14 */
-#ifdef __WIN32__
-#include "bank_defs.h"
-#endif
 
 #include "core/mem_map.h"
 #include "core/hle/hle.h"
@@ -58,325 +33,243 @@ enum {
     THUMB = (1 << 7)
 };
 
-#define USER_MODE_OPT   1
-#define HYBRID_MODE             0 // Enable for JIT mode
+#define USER_MODE_OPT    1
+#define HYBRID_MODE        0 // Enable for JIT mode
 
-#define THRESHOLD                       1000
-#define DURATION                        500
-//#define PRINT_PROFILE_INFO
+#define THRESHOLD            1000
+#define DURATION            500
 
-#define CHECK_RS        if(RS == 15) rs += 8
-#define CHECK_RM        if(RM == 15) rm += 8
+#define CHECK_RS     if(RS == 15) rs += 8
+#define CHECK_RM     if(RM == 15) rm += 8
 
-//#define BITS(s, a, b) (((s) >> (a)) & ((1 << (1 + (b) - (a))) - 1))
 #undef BITS
 #define BITS(s, a, b) ((s << ((sizeof(s) * 8 - 1) - b)) >> (sizeof(s) * 8 - b + a - 1))
 #define BIT(s, n) ((s >> (n)) & 1)
-#define RM      BITS(sht_oper, 0, 3)
-#define RS      BITS(sht_oper, 8, 11)
-
-#define glue(x, y)              x ## y
-#define DPO(s)                  glue(DataProcessingOperands, s)
-#define ROTATE_RIGHT(n, i, l)   ((n << (l - i)) | (n >> i))
-#define ROTATE_LEFT(n, i, l)    ((n >> (l - i)) | (n << i))
-#define ROTATE_RIGHT_32(n, i)   ROTATE_RIGHT(n, i, 32)
-#define ROTATE_LEFT_32(n, i)    ROTATE_LEFT(n, i, 32)
-
-//#define rotr(x,n) ((((x)>>(n))&((1<<(sizeof(x) * 8)-1))|(x<<(sizeof(x)*8-n))))
-//#define rotl(x,n) ((((x)<<(n))&(-(1<<(n))))|(((x)>>(sizeof(x)*8-n))&((1<<(n))-1)))
+#define RM    BITS(sht_oper, 0, 3)
+#define RS    BITS(sht_oper, 8, 11)
+
+#define glue(x, y)        x ## y
+#define DPO(s)            glue(DataProcessingOperands, s)
+#define ROTATE_RIGHT(n, i, l)    ((n << (l - i)) | (n >> i))
+#define ROTATE_LEFT(n, i, l)    ((n >> (l - i)) | (n << i))
+#define ROTATE_RIGHT_32(n, i)    ROTATE_RIGHT(n, i, 32)
+#define ROTATE_LEFT_32(n, i)    ROTATE_LEFT(n, i, 32)
+
 #define rotr(x,n) ( (x >> n) | ((x & ((1 << (n + 1)) - 1)) << (32 - n)) )
 
 extern void switch_mode(arm_core_t *core, uint32_t mode);
-//extern bool InAPrivilegedMode(arm_core_t *core);
 
 typedef arm_core_t arm_processor;
 typedef unsigned int (*shtop_fp_t)(arm_processor *cpu, unsigned int sht_oper);
 
-/* exclusive memory access */
+// Exclusive memory access
 static int exclusive_detect(ARMul_State* state, ARMword addr){
-        #if 0
-        for(int i = 0; i < 128; i++){
-                if(state->exclusive_tag_array[i] == addr)
-                        return 0;
-        }
-        #endif
-        if(state->exclusive_tag == addr)
-                return 0;
-        else
-                return -1;
+    if(state->exclusive_tag == addr)
+        return 0;
+    else
+        return -1;
 }
 
 static void add_exclusive_addr(ARMul_State* state, ARMword addr){
-        #if 0
-        for(int i = 0; i < 128; i++){
-                if(state->exclusive_tag_array[i] == 0xffffffff){
-                        state->exclusive_tag_array[i] = addr;
-                        //DEBUG_LOG(ARM11, "In %s, add  addr 0x%x\n", __func__, addr);
-                        return;
-                }
-        }
-        DEBUG_LOG(ARM11, "In %s ,can not monitor the addr, out of array\n", __FUNCTION__);
-        #endif
-        state->exclusive_tag = addr;
-        return;
+    state->exclusive_tag = addr;
+    return;
 }
 
 static void remove_exclusive(ARMul_State* state, ARMword addr){
-        #if 0
-        int i;
-        for(i = 0; i < 128; i++){
-                if(state->exclusive_tag_array[i] == addr){
-                        state->exclusive_tag_array[i] = 0xffffffff;
-                        //DEBUG_LOG(ARM11, "In %s, remove  addr 0x%x\n", __func__, addr);
-                        return;
-                }
-        }
-        #endif
-        state->exclusive_tag = 0xFFFFFFFF;
-}
-
-
-unsigned int DPO(Immediate)(arm_processor *cpu, unsigned int sht_oper)
-{
-//      DEBUG_LOG(ARM11, "in %s\n", __FUNCTION__);
-        unsigned int immed_8 = BITS(sht_oper, 0, 7);
-        unsigned int rotate_imm = BITS(sht_oper, 8, 11);
-//      DEBUG_LOG(ARM11, "immed_8 is %x\n", immed_8);
-//      DEBUG_LOG(ARM11, "rotate_imm is %x\n", rotate_imm);
-        unsigned int shifter_operand = ROTATE_RIGHT_32(immed_8, rotate_imm * 2);//ROTATE_RIGHT_32(immed_8, rotate_imm * 2);
-//      DEBUG_LOG(ARM11, "shifter_operand : %x\n", shifter_operand);
-        /* set c flag */
-        if (rotate_imm == 0) 
-                cpu->shifter_carry_out = cpu->CFlag;
-        else
-                cpu->shifter_carry_out = BIT(shifter_operand, 31);
-        return shifter_operand;
-}
-
-unsigned int DPO(Register)(arm_processor *cpu, unsigned int sht_oper)
-{
-//      DEBUG_LOG(ARM11, "in %s\n", __FUNCTION__);
-        unsigned int rm = CHECK_READ_REG15(cpu, RM);
-        //if (RM == 15) rm += 8;
-        unsigned int shifter_operand = rm;
-        cpu->shifter_carry_out = cpu->CFlag;
-        return shifter_operand;
+    state->exclusive_tag = 0xFFFFFFFF;
+}
+
+
+unsigned int DPO(Immediate)(arm_processor *cpu, unsigned int sht_oper) {
+    unsigned int immed_8 = BITS(sht_oper, 0, 7);
+    unsigned int rotate_imm = BITS(sht_oper, 8, 11);
+    unsigned int shifter_operand = ROTATE_RIGHT_32(immed_8, rotate_imm * 2);
+    if (rotate_imm == 0) 
+        cpu->shifter_carry_out = cpu->CFlag;
+    else
+        cpu->shifter_carry_out = BIT(shifter_operand, 31);
+    return shifter_operand;
+}
+
+unsigned int DPO(Register)(arm_processor *cpu, unsigned int sht_oper) {
+    unsigned int rm = CHECK_READ_REG15(cpu, RM);
+    unsigned int shifter_operand = rm;
+    cpu->shifter_carry_out = cpu->CFlag;
+    return shifter_operand;
+}
+
+unsigned int DPO(LogicalShiftLeftByImmediate)(arm_processor *cpu, unsigned int sht_oper) {
+    int shift_imm = BITS(sht_oper, 7, 11);
+    unsigned int rm = CHECK_READ_REG15(cpu, RM);
+    unsigned int shifter_operand;
+    if (shift_imm == 0) {
+        shifter_operand = rm;
+        cpu->shifter_carry_out = cpu->CFlag;
+    } else {
+        shifter_operand = rm << shift_imm;
+        cpu->shifter_carry_out = BIT(rm, 32 - shift_imm);
+    }
+    return shifter_operand;
+}
+
+unsigned int DPO(LogicalShiftLeftByRegister)(arm_processor *cpu, unsigned int sht_oper) {
+    int shifter_operand;
+    unsigned int rm = CHECK_READ_REG15(cpu, RM);
+    unsigned int rs = CHECK_READ_REG15(cpu, RS);
+    if (BITS(rs, 0, 7) == 0) {
+        shifter_operand = rm;
+        cpu->shifter_carry_out = cpu->CFlag;
+    } else if (BITS(rs, 0, 7) < 32) {
+        shifter_operand = rm << BITS(rs, 0, 7);
+        cpu->shifter_carry_out = BIT(rm, 32 - BITS(rs, 0, 7));
+    } else if (BITS(rs, 0, 7) == 32) {
+        shifter_operand = 0;
+        cpu->shifter_carry_out = BIT(rm, 0);
+    } else {
+        shifter_operand = 0;
+        cpu->shifter_carry_out = 0;
+    }
+    return shifter_operand;
+}
+
+unsigned int DPO(LogicalShiftRightByImmediate)(arm_processor *cpu, unsigned int sht_oper) {
+    unsigned int rm = CHECK_READ_REG15(cpu, RM);
+    unsigned int shifter_operand;
+    int shift_imm = BITS(sht_oper, 7, 11);
+    if (shift_imm == 0) {
+        shifter_operand = 0;
+        cpu->shifter_carry_out = BIT(rm, 31);
+    } else {
+        shifter_operand = rm >> shift_imm;
+        cpu->shifter_carry_out = BIT(rm, shift_imm - 1);
+    }
+    return shifter_operand;
+}
+
+unsigned int DPO(LogicalShiftRightByRegister)(arm_processor *cpu, unsigned int sht_oper) {
+    unsigned int rs = CHECK_READ_REG15(cpu, RS);
+    unsigned int rm = CHECK_READ_REG15(cpu, RM);
+    unsigned int shifter_operand;
+    if (BITS(rs, 0, 7) == 0) {
+        shifter_operand = rm;
+        cpu->shifter_carry_out = cpu->CFlag;
+    } else if (BITS(rs, 0, 7) < 32) {
+        shifter_operand = rm >> BITS(rs, 0, 7);
+        cpu->shifter_carry_out = BIT(rm, BITS(rs, 0, 7) - 1);
+    } else if (BITS(rs, 0, 7) == 32) {
+        shifter_operand = 0;
+        cpu->shifter_carry_out = BIT(rm, 31);
+    } else {
+        shifter_operand = 0;
+        cpu->shifter_carry_out = 0;
+    }
+    return shifter_operand;
+}
+
+unsigned int DPO(ArithmeticShiftRightByImmediate)(arm_processor *cpu, unsigned int sht_oper) {
+    unsigned int rm = CHECK_READ_REG15(cpu, RM);
+    unsigned int shifter_operand;
+    int shift_imm = BITS(sht_oper, 7, 11);
+    if (shift_imm == 0) {
+        if (BIT(rm, 31)) {
+            shifter_operand = 0;
+            cpu->shifter_carry_out = BIT(rm, 31);
+        } else {
+            shifter_operand = 0xFFFFFFFF;
+            cpu->shifter_carry_out = BIT(rm, 31);
+        }
+    } else {
+        shifter_operand = static_cast<int>(rm) >> shift_imm;
+        cpu->shifter_carry_out = BIT(rm, shift_imm - 1);
+    }
+    return shifter_operand;
+}
+
+unsigned int DPO(ArithmeticShiftRightByRegister)(arm_processor *cpu, unsigned int sht_oper) {
+    unsigned int rs = CHECK_READ_REG15(cpu, RS);
+    unsigned int rm = CHECK_READ_REG15(cpu, RM);
+    unsigned int shifter_operand;
+    if (BITS(rs, 0, 7) == 0) {
+        shifter_operand = rm;
+        cpu->shifter_carry_out = cpu->CFlag;
+    } else if (BITS(rs, 0, 7) < 32) {
+        shifter_operand = static_cast<int>(rm) >> BITS(rs, 0, 7);
+        cpu->shifter_carry_out = BIT(rm, BITS(rs, 0, 7) - 1);
+    } else {
+        if (BIT(rm, 31) == 0)
+            shifter_operand = 0;
+        else
+            shifter_operand = 0xffffffff;
+        cpu->shifter_carry_out = BIT(rm, 31);
+    }
+    return shifter_operand;
+}
+
+unsigned int DPO(RotateRightByImmediate)(arm_processor *cpu, unsigned int sht_oper) {
+    unsigned int shifter_operand;
+    unsigned int rm = CHECK_READ_REG15(cpu, RM);
+    int shift_imm = BITS(sht_oper, 7, 11);
+    if (shift_imm == 0) {
+        shifter_operand = (cpu->CFlag << 31) | (rm >> 1);
+        cpu->shifter_carry_out = BIT(rm, 0);
+    } else {
+        shifter_operand = ROTATE_RIGHT_32(rm, shift_imm);
+        cpu->shifter_carry_out = BIT(rm, shift_imm - 1);
+    }
+    return shifter_operand;
+}
+
+unsigned int DPO(RotateRightByRegister)(arm_processor *cpu, unsigned int sht_oper) {
+    unsigned int rm = CHECK_READ_REG15(cpu, RM);
+    unsigned int rs = CHECK_READ_REG15(cpu, RS);
+    unsigned int shifter_operand;
+    if (BITS(rs, 0, 7) == 0) {
+        shifter_operand = rm;
+        cpu->shifter_carry_out = cpu->CFlag;
+    } else if (BITS(rs, 0, 4) == 0) {
+        shifter_operand = rm;
+        cpu->shifter_carry_out = BIT(rm, 31);
+    } else {
+        shifter_operand = ROTATE_RIGHT_32(rm, BITS(rs, 0, 4));
+        cpu->shifter_carry_out = BIT(rm, BITS(rs, 0, 4) - 1);
+    }
+    return shifter_operand;
 }
 
-unsigned int DPO(LogicalShiftLeftByImmediate)(arm_processor *cpu, unsigned int sht_oper)
-{
-//      DEBUG_LOG(ARM11, "in %s\n", __FUNCTION__);
-        int shift_imm = BITS(sht_oper, 7, 11);
-        unsigned int rm = CHECK_READ_REG15(cpu, RM);
-        //if (RM == 15) rm += 8;
-        unsigned int shifter_operand;
-        if (shift_imm == 0) {
-                shifter_operand = rm;
-                cpu->shifter_carry_out = cpu->CFlag;
-        } else {
-                shifter_operand = rm << shift_imm;
-                cpu->shifter_carry_out = BIT(rm, 32 - shift_imm);
-        }
-        return shifter_operand;
-}
-
-unsigned int DPO(LogicalShiftLeftByRegister)(arm_processor *cpu, unsigned int sht_oper)
-{
-//      DEBUG_LOG(ARM11, "in %s\n", __FUNCTION__);
-        int shifter_operand;
-        unsigned int rm = CHECK_READ_REG15(cpu, RM);
-        unsigned int rs = CHECK_READ_REG15(cpu, RS);
-        //if (RM == 15) rm += 8;
-        //if (RS == 15) rs += 8;
-        if (BITS(rs, 0, 7) == 0) {
-                shifter_operand = rm;
-                cpu->shifter_carry_out = cpu->CFlag;
-        } else if (BITS(rs, 0, 7) < 32) {
-                shifter_operand = rm << BITS(rs, 0, 7);
-                cpu->shifter_carry_out = BIT(rm, 32 - BITS(rs, 0, 7));
-        } else if (BITS(rs, 0, 7) == 32) {
-                shifter_operand = 0;
-                cpu->shifter_carry_out = BIT(rm, 0);
-        } else {
-                shifter_operand = 0;
-                cpu->shifter_carry_out = 0;
-        }
-        return shifter_operand;
-}
-
-unsigned int DPO(LogicalShiftRightByImmediate)(arm_processor *cpu, unsigned int sht_oper)
-{
-//      DEBUG_LOG(ARM11, "in %s\n", __FUNCTION__);
-        //unsigned int rm = cpu->Reg[RM];
-        unsigned int rm = CHECK_READ_REG15(cpu, RM);
-        //if (RM == 15) rm += 8;
-        unsigned int shifter_operand;
-        int shift_imm = BITS(sht_oper, 7, 11);
-        if (shift_imm == 0) {
-                shifter_operand = 0;
-                cpu->shifter_carry_out = BIT(rm, 31);
-        } else {
-                shifter_operand = rm >> shift_imm;
-                cpu->shifter_carry_out = BIT(rm, shift_imm - 1);
-        }
-        return shifter_operand;
-}
-
-unsigned int DPO(LogicalShiftRightByRegister)(arm_processor *cpu, unsigned int sht_oper)
-{
-//      DEBUG_LOG(ARM11, "in %s\n", __FUNCTION__);
-        unsigned int rs = CHECK_READ_REG15(cpu, RS);
-        unsigned int rm = CHECK_READ_REG15(cpu, RM);
-        //if (RS == 15) rs += 8;
-        //if (RM == 15) rm += 8;
-        unsigned int shifter_operand;
-        if (BITS(rs, 0, 7) == 0) {
-                shifter_operand = rm;
-                cpu->shifter_carry_out = cpu->CFlag;
-        } else if (BITS(rs, 0, 7) < 32) {
-                shifter_operand = rm >> BITS(rs, 0, 7);
-                cpu->shifter_carry_out = BIT(rm, BITS(rs, 0, 7) - 1);
-        } else if (BITS(rs, 0, 7) == 32) {
-                shifter_operand = 0;
-                cpu->shifter_carry_out = BIT(rm, 31);
-        } else {
-                shifter_operand = 0;
-                cpu->shifter_carry_out = 0;
-        }
-        return shifter_operand;
-}
-
-unsigned int DPO(ArithmeticShiftRightByImmediate)(arm_processor *cpu, unsigned int sht_oper)
-{
-//      DEBUG_LOG(ARM11, "in %s\n", __FUNCTION__);
-        //unsigned int rm = cpu->Reg[RM];
-        unsigned int rm = CHECK_READ_REG15(cpu, RM);
-        //if (RM == 15) rm += 8;
-        unsigned int shifter_operand;
-        int shift_imm = BITS(sht_oper, 7, 11);
-        if (shift_imm == 0) {
-                if (BIT(rm, 31)) {
-                        shifter_operand = 0;
-                        cpu->shifter_carry_out = BIT(rm, 31);
-                } else {
-                        shifter_operand = 0xFFFFFFFF;
-                        cpu->shifter_carry_out = BIT(rm, 31);
-                }
-        } else {
-                shifter_operand = static_cast<int>(rm) >> shift_imm;
-                cpu->shifter_carry_out = BIT(rm, shift_imm - 1);
-        }
-        return shifter_operand;
-}
-
-unsigned int DPO(ArithmeticShiftRightByRegister)(arm_processor *cpu, unsigned int sht_oper)
-{
-//      DEBUG_LOG(ARM11, "in %s\n", __FUNCTION__);
-        //unsigned int rs = cpu->Reg[RS];
-        unsigned int rs = CHECK_READ_REG15(cpu, RS);
-        //unsigned int rm = cpu->Reg[RM];
-        unsigned int rm = CHECK_READ_REG15(cpu, RM);
-        //if (RS == 15) rs += 8;
-        //if (RM == 15) rm += 8;
-        unsigned int shifter_operand;
-        if (BITS(rs, 0, 7) == 0) {
-                shifter_operand = rm;
-                cpu->shifter_carry_out = cpu->CFlag;
-        } else if (BITS(rs, 0, 7) < 32) {
-                shifter_operand = static_cast<int>(rm) >> BITS(rs, 0, 7);
-                cpu->shifter_carry_out = BIT(rm, BITS(rs, 0, 7) - 1);
-        } else {
-                if (BIT(rm, 31) == 0) {
-                        shifter_operand = 0;
-                } else 
-                        shifter_operand = 0xffffffff;
-                cpu->shifter_carry_out = BIT(rm, 31);
-        }
-        return shifter_operand;
-}
-
-unsigned int DPO(RotateRightByImmediate)(arm_processor *cpu, unsigned int sht_oper)
-{
-//      DEBUG_LOG(ARM11, "in %s\n", __FUNCTION__);
-        unsigned int shifter_operand;
-        //unsigned int rm = cpu->Reg[RM];
-        unsigned int rm = CHECK_READ_REG15(cpu, RM);
-        //if (RM == 15) rm += 8;
-        int shift_imm = BITS(sht_oper, 7, 11);
-        if (shift_imm == 0) {
-                shifter_operand = (cpu->CFlag << 31) | 
-                                        (rm >> 1);
-                cpu->shifter_carry_out = BIT(rm, 0);
-        } else {
-                shifter_operand = ROTATE_RIGHT_32(rm, shift_imm);
-                cpu->shifter_carry_out = BIT(rm, shift_imm - 1);
-        }
-        return shifter_operand;
-}
-
-unsigned int DPO(RotateRightByRegister)(arm_processor *cpu, unsigned int sht_oper)
-{
-//      DEBUG_LOG(ARM11, "in %s\n", __FUNCTION__);
-        unsigned int rm = CHECK_READ_REG15(cpu, RM);
-        //if (RM == 15) rm += 8;
-        unsigned int rs = CHECK_READ_REG15(cpu, RS);
-        //if (RS == 15) rs += 8;
-        unsigned int shifter_operand;
-        if (BITS(rs, 0, 7) == 0) {
-                shifter_operand = rm;
-                cpu->shifter_carry_out = cpu->CFlag;
-        } else if (BITS(rs, 0, 4) == 0) {
-                shifter_operand = rm;
-                cpu->shifter_carry_out = BIT(rm, 31);
-        } else {
-                shifter_operand = ROTATE_RIGHT_32(rm, BITS(rs, 0, 4));
-                cpu->shifter_carry_out = BIT(rm, BITS(rs, 0, 4) - 1);
-        }
-        #if 0
-        if (cpu->icounter >= 20371544) {
-                DEBUG_LOG(ARM11, "in %s\n", __FUNCTION__);
-                DEBUG_LOG(ARM11, "RM:%d\nRS:%d\n", RM, RS);
-                DEBUG_LOG(ARM11, "rm:0x%08x\nrs:0x%08x\n", cpu->Reg[RM], cpu->Reg[RS]);
-        }
-        #endif
-        return shifter_operand;
-}
-
-//typedef unsigned int (*get_addr_fp_t)(arm_processor *cpu);
 typedef struct _MiscImmeData {
-        unsigned int U;
-        unsigned int Rn;
-        unsigned int offset_8;
+    unsigned int U;
+    unsigned int Rn;
+    unsigned int offset_8;
 } MiscLSData;
 
 typedef struct _MiscRegData {
-        unsigned int U;
-        unsigned int Rn;
-        unsigned int Rm;
+    unsigned int U;
+    unsigned int Rn;
+    unsigned int Rm;
 } MiscRegData;
 
 typedef struct _MiscImmePreIdx {
-        unsigned int offset_8;
-        unsigned int U;
-        unsigned int Rn;
+    unsigned int offset_8;
+    unsigned int U;
+    unsigned int Rn;
 } MiscImmePreIdx;
 
 typedef struct _MiscRegPreIdx {
-        unsigned int U;
-        unsigned int Rn;
-        unsigned int Rm;
+    unsigned int U;
+    unsigned int Rn;
+    unsigned int Rm;
 } MiscRegPreIdx;
 
 typedef struct _MiscImmePstIdx {
-        unsigned int offset_8;
-        unsigned int U;
-        unsigned int Rn;
+    unsigned int offset_8;
+    unsigned int U;
+    unsigned int Rn;
 } MIscImmePstIdx;
 
 typedef struct _MiscRegPstIdx {
-        unsigned int Rn;
-        unsigned int Rm;
-        unsigned int U;
+    unsigned int Rn;
+    unsigned int Rm;
+    unsigned int U;
 } MiscRegPstIdx;
 
 typedef struct _LSWordorUnsignedByte {
@@ -384,40 +277,38 @@ typedef struct _LSWordorUnsignedByte {
 
 #if USER_MODE_OPT
 static inline fault_t interpreter_read_memory(addr_t virt_addr, addr_t phys_addr, uint32_t &value, uint32_t size){
-        switch(size) {
-        case 8:
+    switch(size) {
+    case 8:
         value = Memory::Read8(virt_addr);
-                break;
-        case 16:
+        break;
+    case 16:
         value = Memory::Read16(virt_addr);
-                break;
-        case 32:
+        break;
+    case 32:
         value = Memory::Read32(virt_addr);
-                break;
-        }
-        return NO_FAULT;
+        break;
+    }
+    return NO_FAULT;
 }
 
-//static inline void interpreter_write_memory(void *mem_ptr, uint32_t offset, uint32_t value, int size)
-static inline fault_t interpreter_write_memory(addr_t virt_addr, addr_t phys_addr, uint32_t value, uint32_t size)
-{
-        switch(size) {
-        case 8:
+static inline fault_t interpreter_write_memory(addr_t virt_addr, addr_t phys_addr, uint32_t value, uint32_t size) {
+    switch(size) {
+    case 8:
         Memory::Write8(virt_addr, value & 0xff);
-                break;
-        case 16:
-                Memory::Write16(virt_addr, value & 0xffff);
-                break;
-        case 32:
-                Memory::Write32(virt_addr, value);
-                break;
-        }
-        return NO_FAULT;
+        break;
+    case 16:
+        Memory::Write16(virt_addr, value & 0xffff);
+        break;
+    case 32:
+        Memory::Write32(virt_addr, value);
+        break;
+    }
+    return NO_FAULT;
 }
 
-static inline fault_t check_address_validity(arm_core_t *core, addr_t virt_addr, addr_t *phys_addr, uint32_t rw){
-        *phys_addr = virt_addr;
-        return NO_FAULT;
+static inline fault_t check_address_validity(arm_core_t *core, addr_t virt_addr, addr_t *phys_addr, uint32_t rw) {
+    *phys_addr = virt_addr;
+    return NO_FAULT;
 }
 
 #else
@@ -430,736 +321,679 @@ fault_t check_address_validity(arm_core_t *core, addr_t virt_addr, addr_t *phys_
 typedef fault_t (*get_addr_fp_t)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw);
 
 typedef struct _ldst_inst {
-        unsigned int inst;
-        get_addr_fp_t get_addr;
+    unsigned int inst;
+    get_addr_fp_t get_addr;
 } ldst_inst;
 #define DEBUG_MSG LOG_DEBUG(Core_ARM11, "inst is %x", inst); CITRA_IGNORE_EXIT(0)
 
 int CondPassed(arm_processor *cpu, unsigned int cond);
-#define LnSWoUB(s)      glue(LnSWoUB, s)
-#define MLnS(s)         glue(MLnS, s)
-#define LdnStM(s)       glue(LdnStM, s)
-
-#define W_BIT           BIT(inst, 21)
-#define U_BIT           BIT(inst, 23)
-#define I_BIT           BIT(inst, 25)
-#define P_BIT           BIT(inst, 24)
-#define OFFSET_12       BITS(inst, 0, 11)
-fault_t LnSWoUB(ImmediateOffset)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw)
-{
-        unsigned int Rn = BITS(inst, 16, 19);
-        unsigned int addr;
-        fault_t fault;
-        if (U_BIT) {
-                addr = CHECK_READ_REG15_WA(cpu, Rn) + OFFSET_12;
-        } else {
-                addr = CHECK_READ_REG15_WA(cpu, Rn) - OFFSET_12;
-        }
-        //if (Rn == 15) rn += 8;
-        virt_addr = addr;
-        fault = check_address_validity(cpu, addr, &phys_addr, rw);
-        return fault;
-//      return addr;
-}
-
-fault_t LnSWoUB(RegisterOffset)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw)
-{
-        fault_t fault;
-        unsigned int Rn = BITS(inst, 16, 19);
-        unsigned int Rm = BITS(inst, 0, 3);
-        unsigned int rn = CHECK_READ_REG15_WA(cpu, Rn);
-        //if (Rn == 15) rn += 8;
-        unsigned int rm = CHECK_READ_REG15_WA(cpu, Rm);
-        //if (Rm == 15) rm += 8;
-        unsigned int addr;
-        if (U_BIT) {
-                addr = rn + rm;
-        } else {
-                addr = rn - rm;
-        }
-        virt_addr = addr;
-        fault = check_address_validity(cpu, addr, &phys_addr, rw);
-        return fault;
-}
-
-fault_t LnSWoUB(ImmediatePostIndexed)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw)
-{
-        fault_t fault;
-        unsigned int Rn = BITS(inst, 16, 19);
-        unsigned int addr = CHECK_READ_REG15_WA(cpu, Rn);
-        //if (Rn == 15) addr += 8;
+#define LnSWoUB(s)    glue(LnSWoUB, s)
+#define MLnS(s)        glue(MLnS, s)
+#define LdnStM(s)    glue(LdnStM, s)
+
+#define W_BIT        BIT(inst, 21)
+#define U_BIT        BIT(inst, 23)
+#define I_BIT        BIT(inst, 25)
+#define P_BIT        BIT(inst, 24)
+#define OFFSET_12    BITS(inst, 0, 11)
+fault_t LnSWoUB(ImmediateOffset)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw) {
+    unsigned int Rn = BITS(inst, 16, 19);
+    unsigned int addr;
+    fault_t fault;
+    if (U_BIT) {
+        addr = CHECK_READ_REG15_WA(cpu, Rn) + OFFSET_12;
+    } else {
+        addr = CHECK_READ_REG15_WA(cpu, Rn) - OFFSET_12;
+    }
+    virt_addr = addr;
+    fault = check_address_validity(cpu, addr, &phys_addr, rw);
+    return fault;
+}
+
+fault_t LnSWoUB(RegisterOffset)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw) {
+    fault_t fault;
+    unsigned int Rn = BITS(inst, 16, 19);
+    unsigned int Rm = BITS(inst, 0, 3);
+    unsigned int rn = CHECK_READ_REG15_WA(cpu, Rn);
+    unsigned int rm = CHECK_READ_REG15_WA(cpu, Rm);
+    unsigned int addr;
+    if (U_BIT) {
+        addr = rn + rm;
+    } else {
+        addr = rn - rm;
+    }
+    virt_addr = addr;
+    fault = check_address_validity(cpu, addr, &phys_addr, rw);
+    return fault;
+}
+
+fault_t LnSWoUB(ImmediatePostIndexed)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw) {
+    fault_t fault;
+    unsigned int Rn = BITS(inst, 16, 19);
+    unsigned int addr = CHECK_READ_REG15_WA(cpu, Rn);
+    
+    virt_addr = addr;
+    fault = check_address_validity(cpu, addr, &phys_addr, rw);
+    if (fault) return fault;
+
+    if (U_BIT) {
+        cpu->Reg[Rn] += OFFSET_12;
+    } else {
+        cpu->Reg[Rn] -= OFFSET_12;
+    }
+    return fault;
+}
+
+fault_t LnSWoUB(ImmediatePreIndexed)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw) {
+    fault_t fault;
+    unsigned int Rn = BITS(inst, 16, 19);
+    unsigned int addr;
+    if (U_BIT) {
+        addr = CHECK_READ_REG15_WA(cpu, Rn) + OFFSET_12;
+    } else {
+        addr = CHECK_READ_REG15_WA(cpu, Rn) - OFFSET_12;
+    }
+
+    virt_addr = addr;
+    fault = check_address_validity(cpu, addr, &phys_addr, rw);
+    if (fault) return fault;
+
+    if (CondPassed(cpu, BITS(inst, 28, 31))) {
+        cpu->Reg[Rn] = addr;
+    }
+    return fault;
+}
+
+fault_t MLnS(RegisterPreIndexed)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw) {
+    fault_t fault;
+    unsigned int addr;
+    unsigned int Rn = BITS(inst, 16, 19);
+    unsigned int Rm = BITS(inst,  0,  3);
+    unsigned int rn = CHECK_READ_REG15_WA(cpu, Rn);
+    unsigned int rm = CHECK_READ_REG15_WA(cpu, Rm);
+
+    if (U_BIT) {
+        addr = rn + rm;
+    } else
+        addr = rn - rm;
+    if(BIT(inst, 20)){ // L BIT
+    }
+    if(BIT(inst, 6)){ // Sign Bit
+    }
+    if(BIT(inst, 5)){ // Half Bit
+    }
+
+    virt_addr = addr;
+    fault = check_address_validity(cpu, addr, &phys_addr, rw);
+    if (fault) return fault;
 
-        virt_addr = addr;
-        fault = check_address_validity(cpu, addr, &phys_addr, rw);
-        if (fault) return fault;
+    if (CondPassed(cpu, BITS(inst, 28, 31))) {
+        cpu->Reg[Rn] = addr;
+    }
+    return fault;
+}
+
+fault_t LnSWoUB(RegisterPreIndexed)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw) {
+    fault_t fault;
+    unsigned int Rn = BITS(inst, 16, 19);
+    unsigned int Rm = BITS(inst, 0, 3);
+    unsigned int rn = CHECK_READ_REG15_WA(cpu, Rn);
+    unsigned int rm = CHECK_READ_REG15_WA(cpu, Rm);
+    unsigned int addr;
+    if (U_BIT) {
+        addr = rn + rm;
+    } else {
+        addr = rn - rm;
+    }
+    virt_addr = addr;
+    fault = check_address_validity(cpu, addr, &phys_addr, rw);
+    if(fault)
+        return fault;
+    if (CondPassed(cpu, BITS(inst, 28, 31))) {
+        cpu->Reg[Rn] = addr;
+    }
+    return fault;
+}
+fault_t LnSWoUB(ScaledRegisterPreIndexed)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw) {
+    fault_t fault;
+    unsigned int shift = BITS(inst, 5, 6);
+    unsigned int shift_imm = BITS(inst, 7, 11);
+    unsigned int Rn = BITS(inst, 16, 19);
+    unsigned int Rm = BITS(inst, 0, 3);
+    unsigned int index;
+    unsigned int addr;
+
+    unsigned int rm = CHECK_READ_REG15_WA(cpu, Rm);
+    unsigned int rn = CHECK_READ_REG15_WA(cpu, Rn);
+
+    switch (shift) {
+    case 0:
+        index = rm << shift_imm;
+        break;
+    case 1:
+        if (shift_imm == 0) {
+            index = 0;
+        } else {
+            index = rm >> shift_imm;
+        }
+        break;
+    case 2:
+        DEBUG_MSG;
+        break;
+    case 3:
+        DEBUG_MSG;
+        break;
+    }
+    if (U_BIT) {
+        addr = rn + index;
+    } else
+        addr = rn - index;
+    virt_addr = addr;
+    fault = check_address_validity(cpu, addr, &phys_addr, rw);
+    if(fault)
+        return fault;
+    if (CondPassed(cpu, BITS(inst, 28, 31))) {
+        cpu->Reg[Rn] = addr;
+    }
+
+    return fault;
+}
+
+fault_t LnSWoUB(ScaledRegisterPostIndexed)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw) {
+    fault_t fault;
+    unsigned int shift = BITS(inst, 5, 6);
+    unsigned int shift_imm = BITS(inst, 7, 11);
+    unsigned int Rn = BITS(inst, 16, 19);
+    unsigned int Rm = BITS(inst, 0, 3);
+    unsigned int index;
+    unsigned int addr;
+
+    unsigned int rm = CHECK_READ_REG15_WA(cpu, Rm);
+    unsigned int rn = CHECK_READ_REG15_WA(cpu, Rn);
+    addr = rn;
+    switch (shift) {
+    case 0:
+        index = rm << shift_imm;
+        break;
+    case 1:
+        if (shift_imm == 0) {
+            index = 0;
+        } else {
+            index = rm >> shift_imm;
+        }
+        break;
+    case 2:
+        DEBUG_MSG;
+        break;
+    case 3:
+        DEBUG_MSG;
+        break;
+    }
+    virt_addr = addr;
+    fault = check_address_validity(cpu, addr, &phys_addr, rw);
+    if(fault)
+        return fault;
+    if (CondPassed(cpu, BITS(inst, 28, 31))) {
+        if (U_BIT)
+            cpu->Reg[Rn] += index;
+        else
+            cpu->Reg[Rn] -= index;
+    }
 
-        if (U_BIT) {
-                cpu->Reg[Rn] += OFFSET_12;
-        } else {
-                cpu->Reg[Rn] -= OFFSET_12;
-        }
-        return fault;
+    return fault;
 }
 
-fault_t LnSWoUB(ImmediatePreIndexed)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw)
-{
-        fault_t fault;
-        unsigned int Rn = BITS(inst, 16, 19);
-        unsigned int addr;
-        if (U_BIT) {
-                addr = CHECK_READ_REG15_WA(cpu, Rn) + OFFSET_12;
-        } else {
-                addr = CHECK_READ_REG15_WA(cpu, Rn) - OFFSET_12;
-        }
-        #if 0
-        if (Rn == 15) {
-                addr += 8;
-        }
-        #endif
-
-        virt_addr = addr;
-        fault = check_address_validity(cpu, addr, &phys_addr, rw);
-        if (fault) return fault;
-
-        if (CondPassed(cpu, BITS(inst, 28, 31))) {
-                cpu->Reg[Rn] = addr;
-        }
-        return fault;
-}
-
-fault_t MLnS(RegisterPreIndexed)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw)
-{
-        fault_t fault;
-        unsigned int addr;
-        unsigned int Rn = BITS(inst, 16, 19);
-        unsigned int Rm = BITS(inst,  0,  3);
-        unsigned int rn = CHECK_READ_REG15_WA(cpu, Rn);
-        unsigned int rm = CHECK_READ_REG15_WA(cpu, Rm);
-        //if (Rn == 15) rn += 8;
-        //if (Rm == 15) rm += 8;
-        if (U_BIT) {
-                addr = rn + rm;
-        } else
-                addr = rn - rm;
-        if(BIT(inst, 20)){ /* L BIT */
-        }
-        if(BIT(inst, 6)){ /* Sign Bit */
-        }
-        if(BIT(inst, 5)){ /* Half Bit */
-        }
-
-        virt_addr = addr;
-        fault = check_address_validity(cpu, addr, &phys_addr, rw);
-        if (fault) return fault;
-
-        if (CondPassed(cpu, BITS(inst, 28, 31))) {
-                cpu->Reg[Rn] = addr;
-        }
-        return fault;
-}
-
-fault_t LnSWoUB(RegisterPreIndexed)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw)
-{
-        fault_t fault;
-        unsigned int Rn = BITS(inst, 16, 19);
-        unsigned int Rm = BITS(inst, 0, 3);
-        unsigned int rn = CHECK_READ_REG15_WA(cpu, Rn);
-        //if (Rn == 15) rn += 8;
-        unsigned int rm = CHECK_READ_REG15_WA(cpu, Rm);
-        //if (Rm == 15) rm += 8;
-        unsigned int addr;
-        if (U_BIT) {
-                addr = rn + rm;
-        } else {
-                addr = rn - rm;
-        }
-        virt_addr = addr;
-        fault = check_address_validity(cpu, addr, &phys_addr, rw);
-        if(fault)
-                return fault;
-        if (CondPassed(cpu, BITS(inst, 28, 31))) {
-                cpu->Reg[Rn] = addr;
-        }
-        return fault;
-}
-fault_t LnSWoUB(ScaledRegisterPreIndexed)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw)
-{
-        fault_t fault;
-        unsigned int shift = BITS(inst, 5, 6);
-        unsigned int shift_imm = BITS(inst, 7, 11);
-        unsigned int Rn = BITS(inst, 16, 19);
-        unsigned int Rm = BITS(inst, 0, 3);
-        unsigned int index;
-        unsigned int addr;
-
-        unsigned int rm = CHECK_READ_REG15_WA(cpu, Rm);
-        //if (Rm == 15) rm += 8;
-        unsigned int rn = CHECK_READ_REG15_WA(cpu, Rn);
-        //if (Rn == 15) rn += 8;
-        switch (shift) {
-        case 0:
-                //DEBUG_MSG;
-                index = rm << shift_imm;
-                break;
-        case 1:
-//              DEBUG_MSG;
-                if (shift_imm == 0) {
-                        index = 0;
-                } else {
-                        index = rm >> shift_imm;
-                }
-                break;
-        case 2:
-                DEBUG_MSG;
-                break;
-        case 3:
-                DEBUG_MSG;
-                break;
-        }
-        if (U_BIT) {
-                addr = rn + index;
-        } else
-                addr = rn - index;
-        virt_addr = addr;
-        fault = check_address_validity(cpu, addr, &phys_addr, rw);
-        if(fault)
-                return fault;
-        if (CondPassed(cpu, BITS(inst, 28, 31))) {
-                cpu->Reg[Rn] = addr;
-        }
-
-        return fault;
-}
-
-fault_t LnSWoUB(ScaledRegisterPostIndexed)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw)
-{
-        fault_t fault;
-        unsigned int shift = BITS(inst, 5, 6);
-        unsigned int shift_imm = BITS(inst, 7, 11);
-        unsigned int Rn = BITS(inst, 16, 19);
-        unsigned int Rm = BITS(inst, 0, 3);
-        unsigned int index;
-        unsigned int addr;
-
-        unsigned int rm = CHECK_READ_REG15_WA(cpu, Rm);
-        //if (Rm == 15) rm += 8;
-        unsigned int rn = CHECK_READ_REG15_WA(cpu, Rn);
-        //if (Rn == 15) rn += 8;
-        addr = rn;
-        switch (shift) {
-        case 0:
-                //DEBUG_MSG;
-                index = rm << shift_imm;
-                break;
-        case 1:
-//              DEBUG_MSG;
-                if (shift_imm == 0) {
-                        index = 0;
-                } else {
-                        index = rm >> shift_imm;
-                }
-                break;
-        case 2:
-                DEBUG_MSG;
-                break;
-        case 3:
-                DEBUG_MSG;
-                break;
-        }
-        virt_addr = addr;
-        fault = check_address_validity(cpu, addr, &phys_addr, rw);
-        if(fault)
-                return fault;
-        if (CondPassed(cpu, BITS(inst, 28, 31))) {
-                if (U_BIT)
-                        cpu->Reg[Rn] += index;
-                else
-                        cpu->Reg[Rn] -= index;
-        }
-
-        return fault;
-}
-
-fault_t LnSWoUB(RegisterPostIndexed)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw)
-{
-        fault_t fault;
-        unsigned int Rn = BITS(inst, 16, 19);
-        unsigned int Rm = BITS(inst,  0,  3);
-        unsigned int rm = CHECK_READ_REG15_WA(cpu, Rm);
-        unsigned int rn = CHECK_READ_REG15_WA(cpu, Rn);
-
-        unsigned int addr = rn;
-        virt_addr = addr;
-        fault = check_address_validity(cpu, addr, &phys_addr, rw);
-        if (fault) return fault;
-
-        if (CondPassed(cpu, BITS(inst, 28, 31))) {
-                if (U_BIT) {
-                        cpu->Reg[Rn] += rm;
-                } else {
-                        cpu->Reg[Rn] -= rm;
-                }
-        }
-        return fault;
-}
-
-fault_t MLnS(ImmediateOffset)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw)
-{
-        fault_t fault;
-        unsigned int immedL = BITS(inst, 0, 3);
-        unsigned int immedH = BITS(inst, 8, 11);
-
-        unsigned int Rn     = BITS(inst, 16, 19);
-        unsigned int addr;
-
-        unsigned int offset_8 = (immedH << 4) | immedL;
-        if (U_BIT) {
-                addr = CHECK_READ_REG15_WA(cpu, Rn) + offset_8;
-        } else
-                addr = CHECK_READ_REG15_WA(cpu, Rn) - offset_8;
-        #if 0
-        if (Rn == 15) {
-                addr += 8;
-        }
-        #endif
-        virt_addr = addr;
-        fault = check_address_validity(cpu, addr, &phys_addr, rw);
-        return fault;
-}
-
-fault_t MLnS(RegisterOffset)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw)
-{
-        fault_t fault;
-        unsigned int addr;
-        unsigned int Rn = BITS(inst, 16, 19);
-        unsigned int Rm = BITS(inst,  0,  3);
-        unsigned int rn = CHECK_READ_REG15_WA(cpu, Rn);
-        unsigned int rm = CHECK_READ_REG15_WA(cpu, Rm);
-        //if (Rn == 15) rn += 8;
-        //if (Rm == 15) rm += 8;
-        if (U_BIT) {
-                addr = rn + rm;
-        } else
-                addr = rn - rm;
-        if(BIT(inst, 20)){ /* L BIT */
-        }
-        if(BIT(inst, 6)){ /* Sign Bit */
-        }
-        if(BIT(inst, 5)){ /* Half Bit */
-        }
-        virt_addr = addr;
-        fault = check_address_validity(cpu, addr, &phys_addr, rw);
-        return fault;
-}
-
-fault_t MLnS(ImmediatePreIndexed)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw)
-{
-        fault_t fault;
-        unsigned int Rn     = BITS(inst, 16, 19);
-        unsigned int immedH = BITS(inst,  8, 11);
-        unsigned int immedL = BITS(inst,  0,  3);
-        unsigned int addr;
-        unsigned int rn = CHECK_READ_REG15_WA(cpu, Rn);
-        //if (Rn == 15) rn += 8;
-
-//      DEBUG_LOG(ARM11, "in %s\n", __FUNCTION__);
-        unsigned int offset_8 = (immedH << 4) | immedL;
-        if (U_BIT) {
-                addr = rn + offset_8;
-        } else 
-                addr = rn - offset_8;
-
-        virt_addr = addr;
-        fault = check_address_validity(cpu, addr, &phys_addr, rw);
-        if (fault) return fault;
-
-        if (CondPassed(cpu, BITS(inst, 28, 31))) {
-                cpu->Reg[Rn] = addr;
-        }
-        return fault;
-}
-
-fault_t MLnS(ImmediatePostIndexed)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw)
-{
-        fault_t fault;
-        unsigned int Rn     = BITS(inst, 16, 19);
-        unsigned int immedH = BITS(inst,  8, 11);
-        unsigned int immedL = BITS(inst,  0,  3);
-        unsigned int addr;
-        unsigned int rn = CHECK_READ_REG15_WA(cpu, Rn);
-        addr = rn;
-
-        virt_addr = addr;
-        fault = check_address_validity(cpu, addr, &phys_addr, rw);
-        if (fault) return fault;
-
-        if (CondPassed(cpu, BITS(inst, 28, 31))) {
-                unsigned int offset_8 = (immedH << 4) | immedL;
-                if (U_BIT) {
-                        rn += offset_8;
-                } else {
-                        rn -= offset_8;
-                }
-                cpu->Reg[Rn] = rn;
-        }
-
-        return fault;
-}
-fault_t MLnS(RegisterPostIndexed)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw)
-{
-        fault_t fault;
-        unsigned int Rn = BITS(inst, 16, 19);
-        unsigned int Rm = BITS(inst,  0,  3);
-        unsigned int rm = CHECK_READ_REG15_WA(cpu, Rm);
-        unsigned int rn = CHECK_READ_REG15_WA(cpu, Rn);
-
-        unsigned int addr = rn;
-        virt_addr = addr;
-        fault = check_address_validity(cpu, addr, &phys_addr, rw);
-        if (fault) return fault;
-
-        if (CondPassed(cpu, BITS(inst, 28, 31))) {
-                if (U_BIT) {
-                        cpu->Reg[Rn] += rm;
-                } else {
-                        cpu->Reg[Rn] -= rm;
-                }
-        }
-        return fault;
-}
-
-fault_t LdnStM(DecrementBefore)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw)
-{
-        fault_t fault;
-        unsigned int Rn = BITS(inst, 16, 19);
-        unsigned int i = BITS(inst, 0, 15);
-        int count = 0;
-        while(i) {
-                if(i & 1) count ++;
-                i = i >> 1;
-        }
-        unsigned int rn = CHECK_READ_REG15_WA(cpu, Rn);
-        //if (Rn == 15) rn += 8;
-        unsigned int start_addr = rn - count * 4;
-        unsigned int end_addr   = rn - 4;
-
-        fault = check_address_validity(cpu, end_addr,   &phys_addr, rw);
-        virt_addr = end_addr;
-        if (fault) return fault;
-
-        fault = check_address_validity(cpu, start_addr, &phys_addr, rw);
-        virt_addr = start_addr;
-        if (fault) return fault;
-
-        if (CondPassed(cpu, BITS(inst, 28, 31)) && BIT(inst, 21)) {
-                cpu->Reg[Rn] -= count * 4;
-        }
-
-        return fault;
-}
-
-fault_t LdnStM(IncrementBefore)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw)
-{
-        fault_t fault;
-        unsigned int Rn = BITS(inst, 16, 19);
-        unsigned int i = BITS(inst, 0, 15);
-        unsigned int rn = CHECK_READ_REG15_WA(cpu, Rn);
-        //if (Rn == 15) rn += 8;
-        int count = 0;
-        while(i) {
-                if(i & 1) count ++;
-                i = i >> 1;
-        }
-
-        unsigned int start_addr = rn + 4;
-        unsigned int end_addr   = rn + count * 4;
-
-        fault = check_address_validity(cpu, end_addr,   &phys_addr, rw);
-        virt_addr = end_addr;
-        if (fault) return fault;
-
-        fault = check_address_validity(cpu, start_addr, &phys_addr, rw);
-        virt_addr = start_addr;
-        if (fault) return fault;
-
-        if (CondPassed(cpu, BITS(inst, 28, 31)) && BIT(inst, 21)) {
-                cpu->Reg[Rn] += count * 4;
-        }
-        return fault;
-}
-
-fault_t LdnStM(IncrementAfter)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw)
-{
-        fault_t fault;
-        unsigned int Rn = BITS(inst, 16, 19);
-        unsigned int i = BITS(inst, 0, 15);
-        unsigned int rn = CHECK_READ_REG15_WA(cpu, Rn);
-        int count = 0;
-        while(i) {
-                if(i & 1) count ++;
-                i = i >> 1;
-        }
-        //if (Rn == 15) rn += 8;
-        unsigned int start_addr = rn;
-        unsigned int end_addr = rn + count * 4 - 4;
-
-        fault = check_address_validity(cpu, end_addr,   &phys_addr, rw);
-        virt_addr = end_addr;
-        if (fault) return fault;
-
-        fault = check_address_validity(cpu, start_addr, &phys_addr, rw);
-        virt_addr = start_addr;
-        if (fault) return fault;
-
-        if (CondPassed(cpu, BITS(inst, 28, 31)) && BIT(inst, 21)) {
-                cpu->Reg[Rn] += count * 4;
-        }
-        return fault;
-}
-
-fault_t LdnStM(DecrementAfter)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw)
-{
-        fault_t fault;
-        unsigned int Rn = BITS(inst, 16, 19);
-        unsigned int i = BITS(inst, 0, 15);
-        int count = 0;
-        while(i) {
-                if(i & 1) count ++;
-                i = i >> 1;
-        }
-        unsigned int rn = CHECK_READ_REG15_WA(cpu, Rn);
-        //if (Rn == 15) rn += 8;
-        unsigned int start_addr = rn - count * 4 + 4;
-        unsigned int end_addr   = rn;
-
-        fault = check_address_validity(cpu, end_addr,   &phys_addr, rw);
-        virt_addr = end_addr;
-        if (fault) return fault;
-
-        fault = check_address_validity(cpu, start_addr, &phys_addr, rw);
-        if (fault) return fault;
-        virt_addr = start_addr;
-
-        if (CondPassed(cpu, BITS(inst, 28, 31)) && BIT(inst, 21)) {
-                cpu->Reg[Rn] -= count * 4;
-        }
-        return fault;
-}
-
-fault_t LnSWoUB(ScaledRegisterOffset)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw)
-{
-        fault_t fault;
-        unsigned int shift = BITS(inst, 5, 6);
-        unsigned int shift_imm = BITS(inst, 7, 11);
-        unsigned int Rn = BITS(inst, 16, 19);
-        unsigned int Rm = BITS(inst, 0, 3);
-        unsigned int index;
-        unsigned int addr;
-
-        unsigned int rm = CHECK_READ_REG15_WA(cpu, Rm);
-        //if (Rm == 15) rm += 8;
-        unsigned int rn = CHECK_READ_REG15_WA(cpu, Rn);
-        //if (Rn == 15) rn += 8;
-        switch (shift) {
-        case 0:
-                //DEBUG_MSG;
-                index = rm << shift_imm;
-                break;
-        case 1:
-//              DEBUG_MSG;
-                if (shift_imm == 0) {
-                        index = 0;
-                } else {
-                        index = rm >> shift_imm;
-                }
-                break;
-        case 2:
-                if (shift_imm == 0){ /* ASR #32 */
-                        if (rm >> 31)
-                                index = 0xFFFFFFFF;
-                        else
-                                index = 0;
-                }
-                else {
-                        index = static_cast<int>(rm) >> shift_imm;
-                }
-                break;
-        case 3:
-                DEBUG_MSG;
-                break;
-        }
-        if (U_BIT) {
-                addr = rn + index;
-        } else
-                addr = rn - index;
-        virt_addr = addr;
-        fault = check_address_validity(cpu, addr, &phys_addr, rw);
-        return fault;
-}
-
-#define ISNEG(n)        (n < 0)
-#define ISPOS(n)        (n >= 0)
-
-//enum {
-//      COND            = (1 << 0),
-//      NON_BRANCH      = (1 << 1),
-//      DIRECT_BRANCH   = (1 << 2),
-//      INDIRECT_BRANCH = (1 << 3),
-//      CALL            = (1 << 4),
-//      RET             = (1 << 5),
-//      END_OF_PAGE     = (1 << 6),
-//      THUMB           = (1 << 7)
-//};
+fault_t LnSWoUB(RegisterPostIndexed)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw) {
+    fault_t fault;
+    unsigned int Rn = BITS(inst, 16, 19);
+    unsigned int Rm = BITS(inst,  0,  3);
+    unsigned int rm = CHECK_READ_REG15_WA(cpu, Rm);
+    unsigned int rn = CHECK_READ_REG15_WA(cpu, Rn);
+
+    unsigned int addr = rn;
+    virt_addr = addr;
+    fault = check_address_validity(cpu, addr, &phys_addr, rw);
+    if (fault) return fault;
+
+    if (CondPassed(cpu, BITS(inst, 28, 31))) {
+        if (U_BIT) {
+            cpu->Reg[Rn] += rm;
+        } else {
+            cpu->Reg[Rn] -= rm;
+        }
+    }
+    return fault;
+}
+
+fault_t MLnS(ImmediateOffset)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw) {
+    fault_t fault;
+    unsigned int immedL = BITS(inst, 0, 3);
+    unsigned int immedH = BITS(inst, 8, 11);
+
+    unsigned int Rn     = BITS(inst, 16, 19);
+    unsigned int addr;
+
+    unsigned int offset_8 = (immedH << 4) | immedL;
+    if (U_BIT) {
+        addr = CHECK_READ_REG15_WA(cpu, Rn) + offset_8;
+    } else
+        addr = CHECK_READ_REG15_WA(cpu, Rn) - offset_8;
+    
+    virt_addr = addr;
+    fault = check_address_validity(cpu, addr, &phys_addr, rw);
+    return fault;
+}
+
+fault_t MLnS(RegisterOffset)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw) {
+    fault_t fault;
+    unsigned int addr;
+    unsigned int Rn = BITS(inst, 16, 19);
+    unsigned int Rm = BITS(inst,  0,  3);
+    unsigned int rn = CHECK_READ_REG15_WA(cpu, Rn);
+    unsigned int rm = CHECK_READ_REG15_WA(cpu, Rm);
+    if (U_BIT) {
+        addr = rn + rm;
+    } else
+        addr = rn - rm;
+    if(BIT(inst, 20)){ // L BIT
+    }
+    if(BIT(inst, 6)){ // Sign Bit
+    }
+    if(BIT(inst, 5)){ // Half Bit
+    }
+    virt_addr = addr;
+    fault = check_address_validity(cpu, addr, &phys_addr, rw);
+    return fault;
+}
+
+fault_t MLnS(ImmediatePreIndexed)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw) {
+    fault_t fault;
+    unsigned int Rn     = BITS(inst, 16, 19);
+    unsigned int immedH = BITS(inst,  8, 11);
+    unsigned int immedL = BITS(inst,  0,  3);
+    unsigned int addr;
+    unsigned int rn = CHECK_READ_REG15_WA(cpu, Rn);
+    unsigned int offset_8 = (immedH << 4) | immedL;
+
+    if (U_BIT) {
+        addr = rn + offset_8;
+    } else 
+        addr = rn - offset_8;
+
+    virt_addr = addr;
+    fault = check_address_validity(cpu, addr, &phys_addr, rw);
+    if (fault) return fault;
+
+    if (CondPassed(cpu, BITS(inst, 28, 31))) {
+        cpu->Reg[Rn] = addr;
+    }
+    return fault;
+}
+
+fault_t MLnS(ImmediatePostIndexed)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw) {
+    fault_t fault;
+    unsigned int Rn     = BITS(inst, 16, 19);
+    unsigned int immedH = BITS(inst,  8, 11);
+    unsigned int immedL = BITS(inst,  0,  3);
+    unsigned int addr;
+    unsigned int rn = CHECK_READ_REG15_WA(cpu, Rn);
+    addr = rn;
+
+    virt_addr = addr;
+    fault = check_address_validity(cpu, addr, &phys_addr, rw);
+    if (fault) return fault;
+
+    if (CondPassed(cpu, BITS(inst, 28, 31))) {
+        unsigned int offset_8 = (immedH << 4) | immedL;
+        if (U_BIT) {
+            rn += offset_8;
+        } else {
+            rn -= offset_8;
+        }
+        cpu->Reg[Rn] = rn;
+    }
+
+    return fault;
+}
+fault_t MLnS(RegisterPostIndexed)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw) {
+    fault_t fault;
+    unsigned int Rn = BITS(inst, 16, 19);
+    unsigned int Rm = BITS(inst,  0,  3);
+    unsigned int rm = CHECK_READ_REG15_WA(cpu, Rm);
+    unsigned int rn = CHECK_READ_REG15_WA(cpu, Rn);
+    unsigned int addr = rn;
+
+    virt_addr = addr;
+    fault = check_address_validity(cpu, addr, &phys_addr, rw);
+    if (fault) return fault;
+
+    if (CondPassed(cpu, BITS(inst, 28, 31))) {
+        if (U_BIT) {
+            cpu->Reg[Rn] += rm;
+        } else {
+            cpu->Reg[Rn] -= rm;
+        }
+    }
+    return fault;
+}
+
+fault_t LdnStM(DecrementBefore)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw) {
+    fault_t fault;
+    unsigned int Rn = BITS(inst, 16, 19);
+    unsigned int i = BITS(inst, 0, 15);
+    int count = 0;
+    while(i) {
+        if(i & 1) count ++;
+        i = i >> 1;
+    }
+    unsigned int rn = CHECK_READ_REG15_WA(cpu, Rn);
+    unsigned int start_addr = rn - count * 4;
+    unsigned int end_addr   = rn - 4;
+
+    fault = check_address_validity(cpu, end_addr,   &phys_addr, rw);
+    virt_addr = end_addr;
+    if (fault) return fault;
+
+    fault = check_address_validity(cpu, start_addr, &phys_addr, rw);
+    virt_addr = start_addr;
+    if (fault) return fault;
+
+    if (CondPassed(cpu, BITS(inst, 28, 31)) && BIT(inst, 21)) {
+        cpu->Reg[Rn] -= count * 4;
+    }
+
+    return fault;
+}
+
+fault_t LdnStM(IncrementBefore)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw) {
+    fault_t fault;
+    unsigned int Rn = BITS(inst, 16, 19);
+    unsigned int i = BITS(inst, 0, 15);
+    unsigned int rn = CHECK_READ_REG15_WA(cpu, Rn);
+    int count = 0;
+    while(i) {
+        if(i & 1) count ++;
+        i = i >> 1;
+    }
+
+    unsigned int start_addr = rn + 4;
+    unsigned int end_addr   = rn + count * 4;
+
+    fault = check_address_validity(cpu, end_addr,   &phys_addr, rw);
+    virt_addr = end_addr;
+    if (fault) return fault;
+
+    fault = check_address_validity(cpu, start_addr, &phys_addr, rw);
+    virt_addr = start_addr;
+    if (fault) return fault;
+
+    if (CondPassed(cpu, BITS(inst, 28, 31)) && BIT(inst, 21)) {
+        cpu->Reg[Rn] += count * 4;
+    }
+    return fault;
+}
+
+fault_t LdnStM(IncrementAfter)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw) {
+    fault_t fault;
+    unsigned int Rn = BITS(inst, 16, 19);
+    unsigned int i = BITS(inst, 0, 15);
+    unsigned int rn = CHECK_READ_REG15_WA(cpu, Rn);
+    int count = 0;
+    while(i) {
+        if(i & 1) count ++;
+        i = i >> 1;
+    }
+    unsigned int start_addr = rn;
+    unsigned int end_addr = rn + count * 4 - 4;
+
+    fault = check_address_validity(cpu, end_addr,   &phys_addr, rw);
+    virt_addr = end_addr;
+    if (fault) return fault;
+
+    fault = check_address_validity(cpu, start_addr, &phys_addr, rw);
+    virt_addr = start_addr;
+    if (fault) return fault;
+
+    if (CondPassed(cpu, BITS(inst, 28, 31)) && BIT(inst, 21)) {
+        cpu->Reg[Rn] += count * 4;
+    }
+    return fault;
+}
+
+fault_t LdnStM(DecrementAfter)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw) {
+    fault_t fault;
+    unsigned int Rn = BITS(inst, 16, 19);
+    unsigned int i = BITS(inst, 0, 15);
+    int count = 0;
+    while(i) {
+        if(i & 1) count ++;
+        i = i >> 1;
+    }
+    unsigned int rn = CHECK_READ_REG15_WA(cpu, Rn);
+    unsigned int start_addr = rn - count * 4 + 4;
+    unsigned int end_addr   = rn;
+
+    fault = check_address_validity(cpu, end_addr,   &phys_addr, rw);
+    virt_addr = end_addr;
+    if (fault) return fault;
+
+    fault = check_address_validity(cpu, start_addr, &phys_addr, rw);
+    if (fault) return fault;
+    virt_addr = start_addr;
+
+    if (CondPassed(cpu, BITS(inst, 28, 31)) && BIT(inst, 21)) {
+        cpu->Reg[Rn] -= count * 4;
+    }
+    return fault;
+}
+
+fault_t LnSWoUB(ScaledRegisterOffset)(arm_processor *cpu, unsigned int inst, unsigned int &virt_addr, unsigned int &phys_addr, unsigned int rw) {
+    fault_t fault;
+    unsigned int shift = BITS(inst, 5, 6);
+    unsigned int shift_imm = BITS(inst, 7, 11);
+    unsigned int Rn = BITS(inst, 16, 19);
+    unsigned int Rm = BITS(inst, 0, 3);
+    unsigned int index;
+    unsigned int addr;
+
+    unsigned int rm = CHECK_READ_REG15_WA(cpu, Rm);
+    unsigned int rn = CHECK_READ_REG15_WA(cpu, Rn);
+    switch (shift) {
+    case 0:
+        index = rm << shift_imm;
+        break;
+    case 1:
+        if (shift_imm == 0) {
+            index = 0;
+        } else {
+            index = rm >> shift_imm;
+        }
+        break;
+    case 2:
+        if (shift_imm == 0){ // ASR #32
+            if (rm >> 31)
+                index = 0xFFFFFFFF;
+            else
+                index = 0;
+        }
+        else {
+            index = static_cast<int>(rm) >> shift_imm;
+        }
+        break;
+    case 3:
+        DEBUG_MSG;
+        break;
+    }
+    if (U_BIT) {
+        addr = rn + index;
+    } else
+        addr = rn - index;
+
+    virt_addr = addr;
+    fault = check_address_validity(cpu, addr, &phys_addr, rw);
+
+    return fault;
+}
+
+#define ISNEG(n)    (n < 0)
+#define ISPOS(n)    (n >= 0)
 
 typedef struct _arm_inst {
-        unsigned int idx;
-        unsigned int cond;
-        int br;
-        int load_r15;
-        char component[0];
+    unsigned int idx;
+    unsigned int cond;
+    int br;
+    int load_r15;
+    char component[0];
 } arm_inst;
 
+typedef struct generic_arm_inst {
+    u32 Ra;
+    u32 Rm;
+    u32 Rn;
+    u32 Rd;
+    u8 op1;
+    u8 op2;
+} generic_arm_inst;
+
 typedef struct _adc_inst {
-        unsigned int I;
-        unsigned int S;
-        unsigned int Rn;
-        unsigned int Rd;
-        unsigned int shifter_operand;
-        shtop_fp_t shtop_func;
+    unsigned int I;
+    unsigned int S;
+    unsigned int Rn;
+    unsigned int Rd;
+    unsigned int shifter_operand;
+    shtop_fp_t shtop_func;
 } adc_inst;
 
 typedef struct _add_inst {
-        unsigned int I;
-        unsigned int S;
-        unsigned int Rn;
-        unsigned int Rd;
-        unsigned int shifter_operand;
-        shtop_fp_t shtop_func;
+    unsigned int I;
+    unsigned int S;
+    unsigned int Rn;
+    unsigned int Rd;
+    unsigned int shifter_operand;
+    shtop_fp_t shtop_func;
 } add_inst;
 
 typedef struct _orr_inst {
-        unsigned int I;
-        unsigned int S;
-        unsigned int Rn;
-        unsigned int Rd;
-        unsigned int shifter_operand;
-        shtop_fp_t shtop_func;
+    unsigned int I;
+    unsigned int S;
+    unsigned int Rn;
+    unsigned int Rd;
+    unsigned int shifter_operand;
+    shtop_fp_t shtop_func;
 } orr_inst;
 
 typedef struct _and_inst {
-        unsigned int I;
-        unsigned int S;
-        unsigned int Rn;
-        unsigned int Rd;
-        unsigned int shifter_operand;
-        shtop_fp_t shtop_func;
+    unsigned int I;
+    unsigned int S;
+    unsigned int Rn;
+    unsigned int Rd;
+    unsigned int shifter_operand;
+    shtop_fp_t shtop_func;
 } and_inst;
 
 typedef struct _eor_inst {
-        unsigned int I;
-        unsigned int S;
-        unsigned int Rn;
-        unsigned int Rd;
-        unsigned int shifter_operand;
-        shtop_fp_t shtop_func;
+    unsigned int I;
+    unsigned int S;
+    unsigned int Rn;
+    unsigned int Rd;
+    unsigned int shifter_operand;
+    shtop_fp_t shtop_func;
 } eor_inst;
 
 typedef struct _bbl_inst {
-        unsigned int L;
-        int signed_immed_24;
-        unsigned int next_addr;
-        unsigned int jmp_addr;
+    unsigned int L;
+    int signed_immed_24;
+    unsigned int next_addr;
+    unsigned int jmp_addr;
 } bbl_inst;
 
 typedef struct _bx_inst {
-        unsigned int Rm;
+    unsigned int Rm;
 } bx_inst;
 
 typedef struct _blx_inst {
-        union {
-                int32_t signed_immed_24;
-                uint32_t Rm;
-        } val;
-        unsigned int inst;
+    union {
+        int32_t signed_immed_24;
+        uint32_t Rm;
+    } val;
+    unsigned int inst;
 } blx_inst;
 
 typedef struct _clz_inst {
-        unsigned int Rm;
-        unsigned int Rd;
+    unsigned int Rm;
+    unsigned int Rd;
 } clz_inst;
 
 typedef struct _cps_inst {
-        unsigned int imod0;
-        unsigned int imod1;
-        unsigned int mmod;
-        unsigned int A, I, F;
-        unsigned int mode;
+    unsigned int imod0;
+    unsigned int imod1;
+    unsigned int mmod;
+    unsigned int A, I, F;
+    unsigned int mode;
 } cps_inst;
 
 typedef struct _clrex_inst {
 } clrex_inst;
 
 typedef struct _cpy_inst {
-        unsigned int Rm;
-        unsigned int Rd;
+    unsigned int Rm;
+    unsigned int Rd;
 } cpy_inst;
 
 typedef struct _bic_inst {
-        unsigned int I;
-        unsigned int S;
-        unsigned int Rn;
-        unsigned int Rd;
-        unsigned int shifter_operand;
-        shtop_fp_t shtop_func;
+    unsigned int I;
+    unsigned int S;
+    unsigned int Rn;
+    unsigned int Rd;
+    unsigned int shifter_operand;
+    shtop_fp_t shtop_func;
 } bic_inst;
 
 typedef struct _sub_inst {
-        unsigned int I;
-        unsigned int S;
-        unsigned int Rn;
-        unsigned int Rd;
-        unsigned int shifter_operand;
-        shtop_fp_t shtop_func;
+    unsigned int I;
+    unsigned int S;
+    unsigned int Rn;
+    unsigned int Rd;
+    unsigned int shifter_operand;
+    shtop_fp_t shtop_func;
 } sub_inst;
 
 typedef struct _tst_inst {
-        unsigned int I;
-        unsigned int S;
-        unsigned int Rn;
-        unsigned int Rd;
-        unsigned int shifter_operand;
-        shtop_fp_t shtop_func;
+    unsigned int I;
+    unsigned int S;
+    unsigned int Rn;
+    unsigned int Rd;
+    unsigned int shifter_operand;
+    shtop_fp_t shtop_func;
 } tst_inst;
 
 typedef struct _cmn_inst {
-        unsigned int I;
-        //unsigned int S;
-        unsigned int Rn;
-        //unsigned int Rd;
-        unsigned int shifter_operand;
-        shtop_fp_t shtop_func;
+    unsigned int I;
+    unsigned int Rn;
+    unsigned int shifter_operand;
+    shtop_fp_t shtop_func;
 } cmn_inst;
 
 typedef struct _teq_inst {
-        unsigned int I;
-        unsigned int Rn;
-        unsigned int shifter_operand;
-        shtop_fp_t shtop_func;
+    unsigned int I;
+    unsigned int Rn;
+    unsigned int shifter_operand;
+    shtop_fp_t shtop_func;
 } teq_inst;
 
 typedef struct _stm_inst {
-        unsigned int inst;
+    unsigned int inst;
 } stm_inst;
 
 struct bkpt_inst {
 };
 
 struct blx1_inst {
-        unsigned int addr;
+    unsigned int addr;
 };
 
 struct blx2_inst {
-        unsigned int Rm;
+    unsigned int Rm;
 };
 
 typedef struct _stc_inst {
@@ -1169,443 +1003,418 @@ typedef struct _ldc_inst {
 } ldc_inst;
 
 typedef struct _swi_inst {
-        unsigned int num;
+    unsigned int num;
 } swi_inst;
 
 typedef struct _cmp_inst {
-        unsigned int I;
-        unsigned int Rn;
-        unsigned int shifter_operand;
-        shtop_fp_t shtop_func;
+    unsigned int I;
+    unsigned int Rn;
+    unsigned int shifter_operand;
+    shtop_fp_t shtop_func;
 } cmp_inst;
 
 typedef struct _mov_inst {
-        unsigned int I;
-        unsigned int S;
-        unsigned int Rd;
-        unsigned int shifter_operand;
-        shtop_fp_t shtop_func;
+    unsigned int I;
+    unsigned int S;
+    unsigned int Rd;
+    unsigned int shifter_operand;
+    shtop_fp_t shtop_func;
 } mov_inst;
 
 typedef struct _mvn_inst {
-        unsigned int I;
-        unsigned int S;
-        unsigned int Rd;
-        unsigned int shifter_operand;
-        shtop_fp_t shtop_func;
+    unsigned int I;
+    unsigned int S;
+    unsigned int Rd;
+    unsigned int shifter_operand;
+    shtop_fp_t shtop_func;
 } mvn_inst;
 
 typedef struct _rev_inst {
-        unsigned int Rd;
-        unsigned int Rm;
+    unsigned int Rd;
+    unsigned int Rm;
 } rev_inst;
 
 typedef struct _rsb_inst {
-        unsigned int I;
-        unsigned int S;
-        unsigned int Rn;
-        unsigned int Rd;
-        unsigned int shifter_operand;
-        shtop_fp_t shtop_func;
+    unsigned int I;
+    unsigned int S;
+    unsigned int Rn;
+    unsigned int Rd;
+    unsigned int shifter_operand;
+    shtop_fp_t shtop_func;
 } rsb_inst;
 
 typedef struct _rsc_inst {
-        unsigned int I;
-        unsigned int S;
-        unsigned int Rn;
-        unsigned int Rd;
-        unsigned int shifter_operand;
-        shtop_fp_t shtop_func;
+    unsigned int I;
+    unsigned int S;
+    unsigned int Rn;
+    unsigned int Rd;
+    unsigned int shifter_operand;
+    shtop_fp_t shtop_func;
 } rsc_inst;
 
 typedef struct _sbc_inst {
-        unsigned int I;
-        unsigned int S;
-        unsigned int Rn;
-        unsigned int Rd;
-        unsigned int shifter_operand;
-        shtop_fp_t shtop_func;
+    unsigned int I;
+    unsigned int S;
+    unsigned int Rn;
+    unsigned int Rd;
+    unsigned int shifter_operand;
+    shtop_fp_t shtop_func;
 } sbc_inst;
 
 typedef struct _mul_inst {
-        unsigned int S;
-        unsigned int Rd;
-        unsigned int Rs;
-        unsigned int Rm;
+    unsigned int S;
+    unsigned int Rd;
+    unsigned int Rs;
+    unsigned int Rm;
 } mul_inst;
 
 typedef struct _smul_inst {
-        unsigned int Rd;
-        unsigned int Rs;
-        unsigned int Rm;
-        unsigned int x;
-        unsigned int y;
+    unsigned int Rd;
+    unsigned int Rs;
+    unsigned int Rm;
+    unsigned int x;
+    unsigned int y;
 } smul_inst;
 
 typedef struct _umull_inst {
-        unsigned int S;
-        unsigned int RdHi;
-        unsigned int RdLo;
-        unsigned int Rs;
-        unsigned int Rm;
+    unsigned int S;
+    unsigned int RdHi;
+    unsigned int RdLo;
+    unsigned int Rs;
+    unsigned int Rm;
 } umull_inst;
 typedef struct _smlad_inst {
-        unsigned int m;
-        unsigned int Rm;
-        unsigned int Rd;
-        unsigned int Ra;
-        unsigned int Rn;
+    unsigned int m;
+    unsigned int Rm;
+    unsigned int Rd;
+    unsigned int Ra;
+    unsigned int Rn;
 } smlad_inst;
 
 typedef struct _smla_inst {
-        unsigned int x;
-        unsigned int y;
-        unsigned int Rm;
-        unsigned int Rd;
-        unsigned int Rs;
-        unsigned int Rn;
+    unsigned int x;
+    unsigned int y;
+    unsigned int Rm;
+    unsigned int Rd;
+    unsigned int Rs;
+    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;
-        unsigned int Rs;
-        unsigned int RdHi;
-        unsigned int RdLo;
+    unsigned int S;
+    unsigned int Rm;
+    unsigned int Rs;
+    unsigned int RdHi;
+    unsigned int RdLo;
 } umlal_inst;
 
 typedef struct _smlal_inst {
-        unsigned int S;
-        unsigned int Rm;
-        unsigned int Rs;
-        unsigned int RdHi;
-        unsigned int RdLo;
+    unsigned int S;
+    unsigned int Rm;
+    unsigned int Rs;
+    unsigned int RdHi;
+    unsigned int RdLo;
 } smlal_inst;
 
 typedef struct _mla_inst {
-        unsigned int S;
-        unsigned int Rn;
-        unsigned int Rd;
-        unsigned int Rs;
-        unsigned int Rm;
+    unsigned int S;
+    unsigned int Rn;
+    unsigned int Rd;
+    unsigned int Rs;
+    unsigned int Rm;
 } mla_inst;
 
 typedef struct _mrc_inst {
-        unsigned int opcode_1;
-        unsigned int opcode_2;
-        unsigned int cp_num;
-        unsigned int crn;
-        unsigned int crm;
-        unsigned int Rd;
-        unsigned int inst;
+    unsigned int opcode_1;
+    unsigned int opcode_2;
+    unsigned int cp_num;
+    unsigned int crn;
+    unsigned int crm;
+    unsigned int Rd;
+    unsigned int inst;
 } mrc_inst;
 
 typedef struct _mcr_inst {
-        unsigned int opcode_1;
-        unsigned int opcode_2;
-        unsigned int cp_num;
-        unsigned int crn;
-        unsigned int crm;
-        unsigned int Rd;
-        unsigned int inst;
+    unsigned int opcode_1;
+    unsigned int opcode_2;
+    unsigned int cp_num;
+    unsigned int crn;
+    unsigned int crm;
+    unsigned int Rd;
+    unsigned int inst;
 } mcr_inst;
 
 typedef struct _mrs_inst {
-        unsigned int R;
-        unsigned int Rd;
+    unsigned int R;
+    unsigned int Rd;
 } mrs_inst;
 
 typedef struct _msr_inst {
-        unsigned int field_mask;
-        unsigned int R;
-        unsigned int inst;
+    unsigned int field_mask;
+    unsigned int R;
+    unsigned int inst;
 } msr_inst;
 
 typedef struct _pld_inst {
 } pld_inst;
 
 typedef struct _sxtb_inst {
-        unsigned int Rd;
-        unsigned int Rm;
-        unsigned int rotate;
+    unsigned int Rd;
+    unsigned int Rm;
+    unsigned int rotate;
 } sxtb_inst;
 
 typedef struct _sxtab_inst {
-        unsigned int Rd;
-        unsigned int Rn;
-        unsigned int Rm;
-        unsigned rotate;
+    unsigned int Rd;
+    unsigned int Rn;
+    unsigned int Rm;
+    unsigned rotate;
 } sxtab_inst;
 
 typedef struct _sxtah_inst {
-        unsigned int Rd;
-        unsigned int Rn;
-        unsigned int Rm;
-        unsigned int rotate;
+    unsigned int Rd;
+    unsigned int Rn;
+    unsigned int Rm;
+    unsigned int rotate;
 } sxtah_inst;
 
 typedef struct _sxth_inst {
-        unsigned int Rd;
-        unsigned int Rm;
-        unsigned int rotate;
+    unsigned int Rd;
+    unsigned int Rm;
+    unsigned int rotate;
 } sxth_inst;
 
 typedef struct _uxtab_inst {
-        unsigned int Rn;
-        unsigned int Rd;
-        unsigned int rotate;
-        unsigned int Rm;
+    unsigned int Rn;
+    unsigned int Rd;
+    unsigned int rotate;
+    unsigned int Rm;
 } uxtab_inst;
 
 typedef struct _uxtah_inst {
-        unsigned int Rn;
-        unsigned int Rd;
-        unsigned int rotate;
-        unsigned int Rm;
+    unsigned int Rn;
+    unsigned int Rd;
+    unsigned int rotate;
+    unsigned int Rm;
 } uxtah_inst;
 
 typedef struct _uxth_inst {
-        unsigned int Rd;
-        unsigned int Rm;
-        unsigned int rotate;
+    unsigned int Rd;
+    unsigned int Rm;
+    unsigned int rotate;
 } uxth_inst;
 
 typedef struct _cdp_inst {
-        unsigned int opcode_1;
-        unsigned int CRn;
-        unsigned int CRd;
-        unsigned int cp_num;
-        unsigned int opcode_2;
-        unsigned int CRm;
-        uint32 inst;
+    unsigned int opcode_1;
+    unsigned int CRn;
+    unsigned int CRd;
+    unsigned int cp_num;
+    unsigned int opcode_2;
+    unsigned int CRm;
+    uint32 inst;
 }cdp_inst;
 
 typedef struct _uxtb_inst {
-        unsigned int Rd;
-        unsigned int Rm;
-        unsigned int rotate;
+    unsigned int Rd;
+    unsigned int Rm;
+    unsigned int rotate;
 } uxtb_inst;
 
 typedef struct _swp_inst {
-        unsigned int Rn;
-        unsigned int Rd;
-        unsigned int Rm;
+    unsigned int Rn;
+    unsigned int Rd;
+    unsigned int Rm;
 } swp_inst;
 
 typedef struct _b_2_thumb {
-        unsigned int imm;
+    unsigned int imm;
 }b_2_thumb;
 typedef struct _b_cond_thumb {
-        unsigned int imm;
-        unsigned int cond;
+    unsigned int imm;
+    unsigned int cond;
 }b_cond_thumb;
 
 typedef struct _bl_1_thumb {
-        unsigned int imm;
+    unsigned int imm;
 }bl_1_thumb;
 typedef struct _bl_2_thumb {
-        unsigned int imm;
+    unsigned int imm;
 }bl_2_thumb;
 typedef struct _blx_1_thumb {
-        unsigned int imm;
-        unsigned int instr;
+    unsigned int imm;
+    unsigned int instr;
 }blx_1_thumb;
 
+typedef struct _pkh_inst {
+    u32 Rm;
+    u32 Rn;
+    u32 Rd;
+    u8 imm;
+} pkh_inst;
+
 typedef arm_inst * ARM_INST_PTR;
 
-#define CACHE_BUFFER_SIZE       (64 * 1024 * 2000)
+#define CACHE_BUFFER_SIZE    (64 * 1024 * 2000)
 char inst_buf[CACHE_BUFFER_SIZE];
 int top = 0;
-inline void *AllocBuffer(unsigned int size)
-{
-        int start = top;
-        top += size;
-        if (top > CACHE_BUFFER_SIZE) {
-                LOG_ERROR(Core_ARM11, "inst_buf is full");
-                CITRA_IGNORE_EXIT(-1);
-        }
-        return (void *)&inst_buf[start];
-}
-
-int CondPassed(arm_processor *cpu, unsigned int cond)
-{
-        #define NFLAG           cpu->NFlag
-        #define ZFLAG           cpu->ZFlag
-        #define CFLAG           cpu->CFlag
-        #define VFLAG           cpu->VFlag
-        int temp;
-        switch (cond) {
-        case 0x0:
-                temp = ZFLAG;
-                break;
-        case 0x1:   /* NE */
-                temp = !ZFLAG;
-                break;
-        case 0x6:  /* VS */
-                temp = VFLAG;
-                break;
-        case 0x7:        /* VC */
-                temp = !VFLAG;
-                break;
-        case 0x4:         /* MI */
-                temp = NFLAG;
-                break;
-        case 0x5:        /* PL */
-                temp = !NFLAG;
-                break;
-        case 0x2:       /* CS */
-                temp = CFLAG;
-                break;
-        case 0x3:      /* CC */
-                temp = !CFLAG;
-                break;
-        case 0x8:     /* HI */
-                temp = (CFLAG && !ZFLAG);
-                break;
-        case 0x9:    /* LS */
-                temp = (!CFLAG || ZFLAG);
-                break;
-        case 0xa:   /* GE */
-                temp = ((!NFLAG && !VFLAG) || (NFLAG && VFLAG));
-                break;
-        case 0xb:  /* LT */
-                temp = ((NFLAG && !VFLAG) || (!NFLAG && VFLAG));
-                break;
-        case 0xc:  /* GT */
-                temp = ((!NFLAG && !VFLAG && !ZFLAG)
-                        || (NFLAG && VFLAG && !ZFLAG));
-                break;
-        case 0xd:  /* LE */
-                temp = ((NFLAG && !VFLAG) || (!NFLAG && VFLAG))
-                        || ZFLAG;
-                break;
-        case 0xe: /* AL */
-                temp = 1;
-                break;
-        case 0xf:
-//              DEBUG_LOG(ARM11, "inst is %x\n");
-//              DEBUG_LOG(ARM11, "icounter is %lld\n", cpu->icounter);
-//              CITRA_IGNORE_EXIT(-1);
-                temp = 1;
-                break;
-        }
-        return temp;
+inline void *AllocBuffer(unsigned int size) {
+    int start = top;
+    top += size;
+    if (top > CACHE_BUFFER_SIZE) {
+        LOG_ERROR(Core_ARM11, "inst_buf is full");
+        CITRA_IGNORE_EXIT(-1);
+    }
+    return (void *)&inst_buf[start];
+}
+
+int CondPassed(arm_processor *cpu, unsigned int cond) {
+    #define NFLAG        cpu->NFlag
+    #define ZFLAG        cpu->ZFlag
+    #define CFLAG        cpu->CFlag
+    #define VFLAG        cpu->VFlag
+
+    int temp;
+
+    switch (cond) {
+    case 0x0:
+        temp = ZFLAG;
+        break;
+    case 0x1: // NE
+        temp = !ZFLAG;
+        break;
+    case 0x6: // VS
+        temp = VFLAG;
+        break;
+    case 0x7: // VC
+        temp = !VFLAG;
+        break;
+    case 0x4: // MI
+        temp = NFLAG;
+        break;
+    case 0x5: // PL
+        temp = !NFLAG;
+        break;
+    case 0x2: // CS
+        temp = CFLAG;
+        break;
+    case 0x3: // CC
+        temp = !CFLAG;
+        break;
+    case 0x8: // HI
+        temp = (CFLAG && !ZFLAG);
+        break;
+    case 0x9: // LS
+        temp = (!CFLAG || ZFLAG);
+        break;
+    case 0xa: // GE
+        temp = ((!NFLAG && !VFLAG) || (NFLAG && VFLAG));
+        break;
+    case 0xb: // LT
+        temp = ((NFLAG && !VFLAG) || (!NFLAG && VFLAG));
+        break;
+    case 0xc: // GT
+        temp = ((!NFLAG && !VFLAG && !ZFLAG) || (NFLAG && VFLAG && !ZFLAG));
+        break;
+    case 0xd: // LE
+        temp = ((NFLAG && !VFLAG) || (!NFLAG && VFLAG)) || ZFLAG;
+        break;
+    case 0xe: // AL
+        temp = 1;
+        break;
+    case 0xf:
+        temp = 1;
+        break;
+    }
+    return temp;
 }
 
 enum DECODE_STATUS {
-        DECODE_SUCCESS,
-        DECODE_FAILURE
+    DECODE_SUCCESS,
+    DECODE_FAILURE
 };
 
 int decode_arm_instr(uint32_t instr, int32_t *idx);
 
-shtop_fp_t get_shtop(unsigned int inst)
-{
-        if (BIT(inst, 25)) {
-                return DPO(Immediate);
-        } else if (BITS(inst, 4, 11) == 0) {
-                return DPO(Register);
-        } else if (BITS(inst, 4, 6) == 0) {
-                return DPO(LogicalShiftLeftByImmediate);
-        } else if (BITS(inst, 4, 7) == 1) {
-                return DPO(LogicalShiftLeftByRegister);
-        } else if (BITS(inst, 4, 6) == 2) {
-                return DPO(LogicalShiftRightByImmediate);
-        } else if (BITS(inst, 4, 7) == 3) {
-                return DPO(LogicalShiftRightByRegister);
-        } else if (BITS(inst, 4, 6) == 4) {
-                return DPO(ArithmeticShiftRightByImmediate);
-        } else if (BITS(inst, 4, 7) == 5) {
-                return DPO(ArithmeticShiftRightByRegister);
-        } else if (BITS(inst, 4, 6) == 6) {
-                return DPO(RotateRightByImmediate);
-        } else if (BITS(inst, 4, 7) == 7) {
-                return DPO(RotateRightByRegister);
-        }
-        return NULL;
-}
-
-get_addr_fp_t get_calc_addr_op(unsigned int inst)
-{
-        /* 1 */
-        if (BITS(inst, 24, 27) == 5 && BIT(inst, 21) == 0) {
-//              DEBUG_LOG(ARM11, "line is %d", __LINE__);
-                return LnSWoUB(ImmediateOffset);
-        } else if (BITS(inst, 24, 27) == 7 && BIT(inst, 21) == 0 && BITS(inst, 4, 11) == 0) {
-//              DEBUG_MSG;
-//              DEBUG_LOG(ARM11, "line is %d", __LINE__);
-                return LnSWoUB(RegisterOffset);
-        } else if (BITS(inst, 24, 27) == 7 && BIT(inst, 21) == 0 && BIT(inst, 4) == 0) {
-//              DEBUG_MSG;
-//              DEBUG_LOG(ARM11, "line is %d", __LINE__);
-                return LnSWoUB(ScaledRegisterOffset);
-        } else if (BITS(inst, 24, 27) == 5 && BIT(inst, 21) == 1) {
-//              DEBUG_LOG(ARM11, "line is %d", __LINE__);
-                return LnSWoUB(ImmediatePreIndexed);
-        } else if (BITS(inst, 24, 27) == 7 && BIT(inst, 21) == 1 && BITS(inst, 4, 11) == 0) {
-                return LnSWoUB(RegisterPreIndexed);
-        } else if (BITS(inst, 24, 27) == 7 && BIT(inst, 21) == 1 && BIT(inst, 4) == 0) {
-                return LnSWoUB(ScaledRegisterPreIndexed);
-        } else if (BITS(inst, 24, 27) == 4 && BIT(inst, 21) == 0) {
-                return LnSWoUB(ImmediatePostIndexed);
-        } else if (BITS(inst, 24, 27) == 6 && BIT(inst, 21) == 0 && BITS(inst, 4, 11) == 0) {
-//              DEBUG_MSG;
-                return LnSWoUB(RegisterPostIndexed);
-        } else if (BITS(inst, 24, 27) == 6 && BIT(inst, 21) == 0 && BIT(inst, 4) == 0) {
-                return LnSWoUB(ScaledRegisterPostIndexed);
-//              DEBUG_MSG;
-        } else if (BITS(inst, 24, 27) == 1 && BITS(inst, 21, 22) == 2 && BIT(inst, 7) == 1 && BIT(inst, 4) == 1) {
-        /* 2 */
-//              DEBUG_LOG(ARM11, "line is %d", __LINE__);
-                return MLnS(ImmediateOffset);
-//              DEBUG_MSG;
-        } else if (BITS(inst, 24, 27) == 1 && BITS(inst, 21, 22) == 0 && BIT(inst, 7) == 1 && BIT(inst, 4) == 1) {
-//              DEBUG_LOG(ARM11, "line is %d\n", __LINE__);
-                return MLnS(RegisterOffset);
-//              DEBUG_MSG;
-        } else if (BITS(inst, 24, 27) == 1 && BITS(inst, 21, 22) == 3 && BIT(inst, 7) == 1 && BIT(inst, 4) == 1) {
-//              DEBUG_LOG(ARM11, "line is %d\n", __LINE__);
-                return MLnS(ImmediatePreIndexed);
-//              DEBUG_MSG;
-        } else if (BITS(inst, 24, 27) == 1 && BITS(inst, 21, 22) == 1 && BIT(inst, 7) == 1 && BIT(inst, 4) == 1) {
-                return MLnS(RegisterPreIndexed);
-        } else if (BITS(inst, 24, 27) == 0 && BITS(inst, 21, 22) == 2 && BIT(inst, 7) == 1 && BIT(inst, 4) == 1) {
-//              DEBUG_MSG;
-                return MLnS(ImmediatePostIndexed);
-        } else if (BITS(inst, 24, 27) == 0 && BITS(inst, 21, 22) == 0 && BIT(inst, 7) == 1 && BIT(inst, 4) == 1) {
-                //DEBUG_MSG;
-                return MLnS(RegisterPostIndexed);
-        } else if (BITS(inst, 23, 27) == 0x11) {
-        /* 3 */
-//              DEBUG_MSG;
-//              DEBUG_LOG(ARM11, "line is %d", __LINE__);
-                return LdnStM(IncrementAfter);
-        } else if (BITS(inst, 23, 27) == 0x13) {
-//              DEBUG_LOG(ARM11, "line is %d", __LINE__);
-                return LdnStM(IncrementBefore);
-//              DEBUG_MSG;
-        } else if (BITS(inst, 23, 27) == 0x10) {
-//              DEBUG_MSG;
-//              DEBUG_LOG(ARM11, "line is %d", __LINE__);
-                return LdnStM(DecrementAfter);
-        } else if (BITS(inst, 23, 27) == 0x12) {
-//              DEBUG_MSG;
-//              DEBUG_LOG(ARM11, "line is %d", __LINE__);
-                return LdnStM(DecrementBefore);
-        }
-        #if 0
-        DEBUG_LOG(ARM11, "In %s Unknown addressing mode\n", __FUNCTION__);
-        DEBUG_LOG(ARM11, "inst:%x\n", inst);
-        CITRA_IGNORE_EXIT(-1);
-        #endif
-        return NULL;
+shtop_fp_t get_shtop(unsigned int inst) {
+    if (BIT(inst, 25)) {
+        return DPO(Immediate);
+    } else if (BITS(inst, 4, 11) == 0) {
+        return DPO(Register);
+    } else if (BITS(inst, 4, 6) == 0) {
+        return DPO(LogicalShiftLeftByImmediate);
+    } else if (BITS(inst, 4, 7) == 1) {
+        return DPO(LogicalShiftLeftByRegister);
+    } else if (BITS(inst, 4, 6) == 2) {
+        return DPO(LogicalShiftRightByImmediate);
+    } else if (BITS(inst, 4, 7) == 3) {
+        return DPO(LogicalShiftRightByRegister);
+    } else if (BITS(inst, 4, 6) == 4) {
+        return DPO(ArithmeticShiftRightByImmediate);
+    } else if (BITS(inst, 4, 7) == 5) {
+        return DPO(ArithmeticShiftRightByRegister);
+    } else if (BITS(inst, 4, 6) == 6) {
+        return DPO(RotateRightByImmediate);
+    } else if (BITS(inst, 4, 7) == 7) {
+        return DPO(RotateRightByRegister);
+    }
+    return nullptr;
+}
+
+get_addr_fp_t get_calc_addr_op(unsigned int inst) {
+    if (BITS(inst, 24, 27) == 5 && BIT(inst, 21) == 0) {
+        return LnSWoUB(ImmediateOffset);
+    } else if (BITS(inst, 24, 27) == 7 && BIT(inst, 21) == 0 && BITS(inst, 4, 11) == 0) {
+        return LnSWoUB(RegisterOffset);
+    } else if (BITS(inst, 24, 27) == 7 && BIT(inst, 21) == 0 && BIT(inst, 4) == 0) {
+        return LnSWoUB(ScaledRegisterOffset);
+    } else if (BITS(inst, 24, 27) == 5 && BIT(inst, 21) == 1) {
+        return LnSWoUB(ImmediatePreIndexed);
+    } else if (BITS(inst, 24, 27) == 7 && BIT(inst, 21) == 1 && BITS(inst, 4, 11) == 0) {
+        return LnSWoUB(RegisterPreIndexed);
+    } else if (BITS(inst, 24, 27) == 7 && BIT(inst, 21) == 1 && BIT(inst, 4) == 0) {
+        return LnSWoUB(ScaledRegisterPreIndexed);
+    } else if (BITS(inst, 24, 27) == 4 && BIT(inst, 21) == 0) {
+        return LnSWoUB(ImmediatePostIndexed);
+    } else if (BITS(inst, 24, 27) == 6 && BIT(inst, 21) == 0 && BITS(inst, 4, 11) == 0) {
+        return LnSWoUB(RegisterPostIndexed);
+    } else if (BITS(inst, 24, 27) == 6 && BIT(inst, 21) == 0 && BIT(inst, 4) == 0) {
+        return LnSWoUB(ScaledRegisterPostIndexed);
+    } else if (BITS(inst, 24, 27) == 1 && BITS(inst, 21, 22) == 2 && BIT(inst, 7) == 1 && BIT(inst, 4) == 1) {
+        return MLnS(ImmediateOffset);
+    } else if (BITS(inst, 24, 27) == 1 && BITS(inst, 21, 22) == 0 && BIT(inst, 7) == 1 && BIT(inst, 4) == 1) {
+        return MLnS(RegisterOffset);
+    } else if (BITS(inst, 24, 27) == 1 && BITS(inst, 21, 22) == 3 && BIT(inst, 7) == 1 && BIT(inst, 4) == 1) {
+        return MLnS(ImmediatePreIndexed);
+    } else if (BITS(inst, 24, 27) == 1 && BITS(inst, 21, 22) == 1 && BIT(inst, 7) == 1 && BIT(inst, 4) == 1) {
+        return MLnS(RegisterPreIndexed);
+    } else if (BITS(inst, 24, 27) == 0 && BITS(inst, 21, 22) == 2 && BIT(inst, 7) == 1 && BIT(inst, 4) == 1) {
+        return MLnS(ImmediatePostIndexed);
+    } else if (BITS(inst, 24, 27) == 0 && BITS(inst, 21, 22) == 0 && BIT(inst, 7) == 1 && BIT(inst, 4) == 1) {
+        return MLnS(RegisterPostIndexed);
+    } else if (BITS(inst, 23, 27) == 0x11) {
+        return LdnStM(IncrementAfter);
+    } else if (BITS(inst, 23, 27) == 0x13) {
+        return LdnStM(IncrementBefore);
+    } else if (BITS(inst, 23, 27) == 0x10) {
+        return LdnStM(DecrementAfter);
+    } else if (BITS(inst, 23, 27) == 0x12) {
+        return LdnStM(DecrementBefore);
+    }
+    return nullptr;
 }
 
 #define INTERPRETER_TRANSLATE(s) glue(InterpreterTranslate_, s)
 
-#define CHECK_RN                        (inst_cream->Rn == 15)
-#define CHECK_RM                        (inst_cream->Rm == 15)
-#define CHECK_RS                        (inst_cream->Rs == 15)
+#define CHECK_RN            (inst_cream->Rn == 15)
+#define CHECK_RM            (inst_cream->Rm == 15)
+#define CHECK_RS            (inst_cream->Rs == 15)
 
 #define UNIMPLEMENTED_INSTRUCTION(mnemonic) \
     LOG_ERROR(Core_ARM11, "unimplemented instruction: %s", mnemonic); \
@@ -1614,882 +1423,970 @@ get_addr_fp_t get_calc_addr_op(unsigned int inst)
 
 ARM_INST_PTR INTERPRETER_TRANSLATE(adc)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(adc_inst));
-        adc_inst *inst_cream = (adc_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->I    = BIT(inst, 25);
-        inst_cream->S    = BIT(inst, 20);
-        inst_cream->Rn   = BITS(inst, 16, 19);
-        inst_cream->Rd   = BITS(inst, 12, 15);
-        if (CHECK_RN) 
-                inst_base->load_r15 = 1;
-        inst_cream->shifter_operand = BITS(inst, 0, 11);
-        inst_cream->shtop_func = get_shtop(inst);
-        if (inst_cream->Rd == 15) {
-                inst_base->br = INDIRECT_BRANCH;
-        }
-        return inst_base;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(adc_inst));
+    adc_inst *inst_cream = (adc_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->I     = BIT(inst, 25);
+    inst_cream->S     = BIT(inst, 20);
+    inst_cream->Rn     = BITS(inst, 16, 19);
+    inst_cream->Rd     = BITS(inst, 12, 15);
+    if (CHECK_RN) 
+        inst_base->load_r15 = 1;
+    inst_cream->shifter_operand = BITS(inst, 0, 11);
+    inst_cream->shtop_func = get_shtop(inst);
+    if (inst_cream->Rd == 15) {
+        inst_base->br = INDIRECT_BRANCH;
+    }
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(add)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(add_inst));
-        add_inst *inst_cream = (add_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->I    = BIT(inst, 25);
-        inst_cream->S    = BIT(inst, 20);
-        inst_cream->Rn   = BITS(inst, 16, 19);
-        inst_cream->Rd   = BITS(inst, 12, 15);
-        if (CHECK_RN) 
-                inst_base->load_r15 = 1;
-        inst_cream->shifter_operand = BITS(inst, 0, 11);
-        inst_cream->shtop_func = get_shtop(inst);
-        if (inst_cream->Rd == 15) {
-                inst_base->br = INDIRECT_BRANCH;
-        }
-        return inst_base;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(add_inst));
+    add_inst *inst_cream = (add_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->I     = BIT(inst, 25);
+    inst_cream->S     = BIT(inst, 20);
+    inst_cream->Rn     = BITS(inst, 16, 19);
+    inst_cream->Rd     = BITS(inst, 12, 15);
+    if (CHECK_RN) 
+        inst_base->load_r15 = 1;
+    inst_cream->shifter_operand = BITS(inst, 0, 11);
+    inst_cream->shtop_func = get_shtop(inst);
+    if (inst_cream->Rd == 15) {
+        inst_base->br = INDIRECT_BRANCH;
+    }
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(and)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(and_inst));
-        and_inst *inst_cream = (and_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->I    = BIT(inst, 25);
-        inst_cream->S    = BIT(inst, 20);
-        inst_cream->Rn   = BITS(inst, 16, 19);
-        inst_cream->Rd   = BITS(inst, 12, 15);
-        if (CHECK_RN) 
-                inst_base->load_r15 = 1;
-        inst_cream->shifter_operand = BITS(inst, 0, 11);
-        inst_cream->shtop_func = get_shtop(inst);
-        if (inst_cream->Rd == 15) 
-                inst_base->br = INDIRECT_BRANCH;
-        return inst_base;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(and_inst));
+    and_inst *inst_cream = (and_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->I     = BIT(inst, 25);
+    inst_cream->S     = BIT(inst, 20);
+    inst_cream->Rn     = BITS(inst, 16, 19);
+    inst_cream->Rd     = BITS(inst, 12, 15);
+    if (CHECK_RN) 
+        inst_base->load_r15 = 1;
+    inst_cream->shifter_operand = BITS(inst, 0, 11);
+    inst_cream->shtop_func = get_shtop(inst);
+    if (inst_cream->Rd == 15) 
+        inst_base->br = INDIRECT_BRANCH;
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(bbl)(unsigned int inst, int index)
 {
-        #define POSBRANCH ((inst & 0x7fffff) << 2)
-        #define NEGBRANCH ((0xff000000 |(inst & 0xffffff)) << 2)
+    #define POSBRANCH ((inst & 0x7fffff) << 2)
+    #define NEGBRANCH ((0xff000000 |(inst & 0xffffff)) << 2)
 
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(bbl_inst));
-        bbl_inst *inst_cream = (bbl_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(bbl_inst));
+    bbl_inst *inst_cream = (bbl_inst *)inst_base->component;
 
-        inst_base->cond  = BITS(inst, 28, 31);
-        inst_base->idx   = index;
-        inst_base->br    = DIRECT_BRANCH;
+    inst_base->cond  = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = DIRECT_BRANCH;
 
-        if (BIT(inst, 24))
-                inst_base->br = CALL;
-        if (BITS(inst, 28, 31) <= 0xe)
-                inst_base->br |= COND;
+    if (BIT(inst, 24))
+        inst_base->br = CALL;
+    if (BITS(inst, 28, 31) <= 0xe)
+        inst_base->br |= COND;
 
-        inst_cream->L    = BIT(inst, 24);
-        inst_cream->signed_immed_24 = BIT(inst, 23) ? NEGBRANCH : POSBRANCH;
+    inst_cream->L      = BIT(inst, 24);
+    inst_cream->signed_immed_24 = BIT(inst, 23) ? NEGBRANCH : POSBRANCH;
 
-        return inst_base;
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(bic)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(bic_inst));
-        bic_inst *inst_cream = (bic_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->I    = BIT(inst, 25);
-        inst_cream->S    = BIT(inst, 20);
-        inst_cream->Rn   = BITS(inst, 16, 19);
-        inst_cream->Rd   = BITS(inst, 12, 15);
-        if (CHECK_RN) 
-                inst_base->load_r15 = 1;
-        inst_cream->shifter_operand = BITS(inst, 0, 11);
-        inst_cream->shtop_func = get_shtop(inst);
-
-        if (inst_cream->Rd == 15) 
-                inst_base->br = INDIRECT_BRANCH;
-        return inst_base;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(bic_inst));
+    bic_inst *inst_cream = (bic_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->I     = BIT(inst, 25);
+    inst_cream->S     = BIT(inst, 20);
+    inst_cream->Rn     = BITS(inst, 16, 19);
+    inst_cream->Rd     = BITS(inst, 12, 15);
+    if (CHECK_RN) 
+        inst_base->load_r15 = 1;
+    inst_cream->shifter_operand = BITS(inst, 0, 11);
+    inst_cream->shtop_func = get_shtop(inst);
+
+    if (inst_cream->Rd == 15) 
+        inst_base->br = INDIRECT_BRANCH;
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(bkpt)(unsigned int inst, int index) { UNIMPLEMENTED_INSTRUCTION("BKPT"); }
 ARM_INST_PTR INTERPRETER_TRANSLATE(blx)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(blx_inst));
-        blx_inst *inst_cream = (blx_inst *)inst_base->component;
-
-        inst_base->cond  = BITS(inst, 28, 31);
-        inst_base->idx   = index;
-        inst_base->br    = INDIRECT_BRANCH;
-
-        inst_cream->inst = inst;
-        if (BITS(inst, 20, 27) == 0x12 && BITS(inst, 4, 7) == 0x3) {
-                inst_cream->val.Rm = BITS(inst, 0, 3);
-        } else {
-                inst_cream->val.signed_immed_24 = BITS(inst, 0, 23);
-                //DEBUG_LOG(ARM11, " blx inst is %x\n", inst);
-                //CITRA_IGNORE_EXIT(-1);
-//              DEBUG_MSG;
-        }
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(blx_inst));
+    blx_inst *inst_cream = (blx_inst *)inst_base->component;
+
+    inst_base->cond  = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = INDIRECT_BRANCH;
+
+    inst_cream->inst = inst;
+    if (BITS(inst, 20, 27) == 0x12 && BITS(inst, 4, 7) == 0x3) {
+        inst_cream->val.Rm = BITS(inst, 0, 3);
+    } else {
+        inst_cream->val.signed_immed_24 = BITS(inst, 0, 23);
+    }
 
-        return inst_base;
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(bx)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(bx_inst));
-        bx_inst *inst_cream = (bx_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(bx_inst));
+    bx_inst *inst_cream = (bx_inst *)inst_base->component;
 
-        inst_base->cond  = BITS(inst, 28, 31);
-        inst_base->idx   = index;
-        inst_base->br    = INDIRECT_BRANCH;
+    inst_base->cond  = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = INDIRECT_BRANCH;
 
-        inst_cream->Rm   = BITS(inst, 0, 3);
+    inst_cream->Rm     = BITS(inst, 0, 3);
 
-        return inst_base;
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(bxj)(unsigned int inst, int index) { UNIMPLEMENTED_INSTRUCTION("BXJ"); }
 ARM_INST_PTR INTERPRETER_TRANSLATE(cdp)(unsigned int inst, int index){
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(cdp_inst));
-        cdp_inst *inst_cream = (cdp_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->CRm   = BITS(inst,  0,  3);
-        inst_cream->CRd   = BITS(inst, 12, 15);
-        inst_cream->CRn   = BITS(inst, 16, 19);
-        inst_cream->cp_num   = BITS(inst, 8, 11);
-        inst_cream->opcode_2   = BITS(inst, 5, 7);
-        inst_cream->opcode_1   = BITS(inst, 20, 23);
-        inst_cream->inst = inst;
-
-        LOG_TRACE(Core_ARM11, "inst %x index %x", inst, index);
-        return inst_base;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(cdp_inst));
+    cdp_inst *inst_cream = (cdp_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->CRm   = BITS(inst,  0,  3);
+    inst_cream->CRd   = BITS(inst, 12, 15);
+    inst_cream->CRn   = BITS(inst, 16, 19);
+    inst_cream->cp_num   = BITS(inst, 8, 11);
+    inst_cream->opcode_2   = BITS(inst, 5, 7);
+    inst_cream->opcode_1   = BITS(inst, 20, 23);
+    inst_cream->inst = inst;
+
+    LOG_TRACE(Core_ARM11, "inst %x index %x", inst, index);
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(clrex)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(clrex_inst));
-        inst_base->cond  = BITS(inst, 28, 31);
-        inst_base->idx   = index;
-        inst_base->br    = NON_BRANCH;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(clrex_inst));
+    inst_base->cond  = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
 
-        return inst_base;
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(clz)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(clz_inst));
-        clz_inst *inst_cream = (clz_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(clz_inst));
+    clz_inst *inst_cream = (clz_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_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,  0,  3);
-        inst_cream->Rd   = BITS(inst, 12, 15);
-        if (CHECK_RM) 
-                inst_base->load_r15 = 1;
+    inst_cream->Rm   = BITS(inst,  0,  3);
+    inst_cream->Rd   = BITS(inst, 12, 15);
+    if (CHECK_RM) 
+        inst_base->load_r15 = 1;
 
-        return inst_base;
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(cmn)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(cmn_inst));
-        cmn_inst *inst_cream = (cmn_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->I    = BIT(inst, 25);
-        //inst_cream->S  = BIT(inst, 20);
-        inst_cream->Rn   = BITS(inst, 16, 19);
-        //inst_cream->Rd         = BITS(inst, 12, 15);
-        if (CHECK_RN) 
-                inst_base->load_r15 = 1;
-        inst_cream->shifter_operand = BITS(inst, 0, 11);
-        inst_cream->shtop_func = get_shtop(inst);
-        return inst_base;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(cmn_inst));
+    cmn_inst *inst_cream = (cmn_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->I     = BIT(inst, 25);
+    //inst_cream->S     = BIT(inst, 20);
+    inst_cream->Rn     = BITS(inst, 16, 19);
+    //inst_cream->Rd     = BITS(inst, 12, 15);
+    if (CHECK_RN) 
+        inst_base->load_r15 = 1;
+    inst_cream->shifter_operand = BITS(inst, 0, 11);
+    inst_cream->shtop_func = get_shtop(inst);
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(cmp)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(cmp_inst));
-        cmp_inst *inst_cream = (cmp_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->I    = BIT(inst, 25);
-        inst_cream->Rn   = BITS(inst, 16, 19);
-        if (CHECK_RN) 
-                inst_base->load_r15 = 1;
-        inst_cream->shifter_operand = BITS(inst, 0, 11);
-        inst_cream->shtop_func = get_shtop(inst);
-        return inst_base;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(cmp_inst));
+    cmp_inst *inst_cream = (cmp_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->I     = BIT(inst, 25);
+    inst_cream->Rn     = BITS(inst, 16, 19);
+    if (CHECK_RN) 
+        inst_base->load_r15 = 1;
+    inst_cream->shifter_operand = BITS(inst, 0, 11);
+    inst_cream->shtop_func = get_shtop(inst);
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(cps)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(cps_inst));
-        cps_inst *inst_cream = (cps_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(cps_inst));
+    cps_inst *inst_cream = (cps_inst *)inst_base->component;
 
-        inst_base->cond  = BITS(inst, 28, 31);
-        inst_base->idx   = index;
-        inst_base->br    = NON_BRANCH;
+    inst_base->cond  = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
 
-        inst_cream->imod0 = BIT(inst, 18);
-        inst_cream->imod1 = BIT(inst, 19);
-        inst_cream->mmod  = BIT(inst, 17);
-        inst_cream->A     = BIT(inst, 8);
-        inst_cream->I     = BIT(inst, 7);
-        inst_cream->F     = BIT(inst, 6);
-        inst_cream->mode  = BITS(inst, 0, 4);
+    inst_cream->imod0 = BIT(inst, 18);
+    inst_cream->imod1 = BIT(inst, 19);
+    inst_cream->mmod  = BIT(inst, 17);
+    inst_cream->A      = BIT(inst, 8);
+    inst_cream->I      = BIT(inst, 7);
+    inst_cream->F      = BIT(inst, 6);
+    inst_cream->mode  = BITS(inst, 0, 4);
 
-        return inst_base;
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(cpy)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(mov_inst));
-        mov_inst *inst_cream = (mov_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(mov_inst));
+    mov_inst *inst_cream = (mov_inst *)inst_base->component;
 
-        inst_base->cond  = BITS(inst, 28, 31);
-        inst_base->idx   = index;
-        inst_base->br    = NON_BRANCH;
+    inst_base->cond  = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
 
-        inst_cream->I    = BIT(inst, 25);
-        inst_cream->S    = BIT(inst, 20);
-        inst_cream->Rd   = BITS(inst, 12, 15);
-        inst_cream->shifter_operand = BITS(inst, 0, 11);
-        inst_cream->shtop_func = get_shtop(inst);
+    inst_cream->I     = BIT(inst, 25);
+    inst_cream->S     = BIT(inst, 20);
+    inst_cream->Rd     = BITS(inst, 12, 15);
+    inst_cream->shifter_operand = BITS(inst, 0, 11);
+    inst_cream->shtop_func = get_shtop(inst);
 
-        if (inst_cream->Rd == 15) {
-                inst_base->br = INDIRECT_BRANCH;
-        }
-        return inst_base;
+    if (inst_cream->Rd == 15) {
+        inst_base->br = INDIRECT_BRANCH;
+    }
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(eor)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(eor_inst));
-        eor_inst *inst_cream = (eor_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->I    = BIT(inst, 25);
-        inst_cream->S    = BIT(inst, 20);
-        inst_cream->Rn   = BITS(inst, 16, 19);
-        inst_cream->Rd   = BITS(inst, 12, 15);
-        if (CHECK_RN) 
-                inst_base->load_r15 = 1;
-        inst_cream->shifter_operand = BITS(inst, 0, 11);
-        inst_cream->shtop_func = get_shtop(inst);
-        if (inst_cream->Rd == 15) {
-                inst_base->br = INDIRECT_BRANCH;
-        }
-        return inst_base;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(eor_inst));
+    eor_inst *inst_cream = (eor_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->I     = BIT(inst, 25);
+    inst_cream->S     = BIT(inst, 20);
+    inst_cream->Rn     = BITS(inst, 16, 19);
+    inst_cream->Rd     = BITS(inst, 12, 15);
+    if (CHECK_RN) 
+        inst_base->load_r15 = 1;
+    inst_cream->shifter_operand = BITS(inst, 0, 11);
+    inst_cream->shtop_func = get_shtop(inst);
+    if (inst_cream->Rd == 15) {
+        inst_base->br = INDIRECT_BRANCH;
+    }
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(ldc)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldc_inst));
-        inst_base->cond  = BITS(inst, 28, 31);
-        inst_base->idx   = index;
-        inst_base->br    = NON_BRANCH;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldc_inst));
+    inst_base->cond  = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
 
-        return inst_base;
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(ldm)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
-        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
+    ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
 
-        inst_base->cond = BITS(inst, 28, 31);
-        inst_base->idx   = index;
-        inst_base->br    = NON_BRANCH;
+    inst_base->cond = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
 
-        inst_cream->inst = inst;
-        inst_cream->get_addr = get_calc_addr_op(inst);
+    inst_cream->inst = inst;
+    inst_cream->get_addr = get_calc_addr_op(inst);
 
-        if (BIT(inst, 15)) {
-                inst_base->br = INDIRECT_BRANCH;
-        }
-        return inst_base;
+    if (BIT(inst, 15)) {
+        inst_base->br = INDIRECT_BRANCH;
+    }
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(sxth)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(sxtb_inst));
-        sxtb_inst *inst_cream = (sxtb_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(sxtb_inst));
+    sxtb_inst *inst_cream = (sxtb_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_base->cond  = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
+    inst_base->load_r15 = 0;
 
-        inst_cream->Rd     = BITS(inst, 12, 15);
-        inst_cream->Rm     = BITS(inst,  0,  3);
-        inst_cream->rotate = BITS(inst, 10, 11);
-        if (CHECK_RM) 
-                inst_base->load_r15 = 1;
+    inst_cream->Rd     = BITS(inst, 12, 15);
+    inst_cream->Rm     = BITS(inst,  0,  3);
+    inst_cream->rotate = BITS(inst, 10, 11);
+    if (CHECK_RM) 
+        inst_base->load_r15 = 1;
 
-        return inst_base;
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(ldr)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
-        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
+    ldst_inst *inst_cream = (ldst_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_base->cond = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
+    inst_base->load_r15 = 0;
 
-        inst_cream->inst = inst;
-        inst_cream->get_addr = get_calc_addr_op(inst);
+    inst_cream->inst = inst;
+    inst_cream->get_addr = get_calc_addr_op(inst);
 
-        if (BITS(inst, 12, 15) == 15) {
-                inst_base->br = INDIRECT_BRANCH;
-        }
-        return inst_base;
+    if (BITS(inst, 12, 15) == 15) {
+        inst_base->br = INDIRECT_BRANCH;
+    }
+    return inst_base;
 }
 
 ARM_INST_PTR INTERPRETER_TRANSLATE(ldrcond)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
-        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
+    ldst_inst *inst_cream = (ldst_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_base->cond = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
+    inst_base->load_r15 = 0;
 
-        inst_cream->inst = inst;
-        inst_cream->get_addr = get_calc_addr_op(inst);
+    inst_cream->inst = inst;
+    inst_cream->get_addr = get_calc_addr_op(inst);
 
-        if (BITS(inst, 12, 15) == 15) {
-                inst_base->br = INDIRECT_BRANCH;
-        }
-        return inst_base;
+    if (BITS(inst, 12, 15) == 15) {
+        inst_base->br = INDIRECT_BRANCH;
+    }
+    return inst_base;
 }
 
 ARM_INST_PTR INTERPRETER_TRANSLATE(uxth)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(uxth_inst));
-        uxth_inst *inst_cream = (uxth_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(uxth_inst));
+    uxth_inst *inst_cream = (uxth_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_base->cond = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
+    inst_base->load_r15 = 0;
 
-        inst_cream->Rd     = BITS(inst, 12, 15);
-        inst_cream->rotate = BITS(inst, 10, 11);
-        inst_cream->Rm     = BITS(inst,  0,  3);
-        if (CHECK_RM) 
-                inst_base->load_r15 = 1;
+    inst_cream->Rd     = BITS(inst, 12, 15);
+    inst_cream->rotate = BITS(inst, 10, 11);
+    inst_cream->Rm     = BITS(inst,  0,  3);
+    if (CHECK_RM) 
+        inst_base->load_r15 = 1;
 
-        return inst_base;
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(uxtah)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(uxtah_inst));
-        uxtah_inst *inst_cream = (uxtah_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(uxtah_inst));
+    uxtah_inst *inst_cream = (uxtah_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_base->cond = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
+    inst_base->load_r15 = 0;
 
-        inst_cream->Rn     = BITS(inst, 16, 19);
-        inst_cream->Rd     = BITS(inst, 12, 15);
-        inst_cream->rotate = BITS(inst, 10, 11);
-        inst_cream->Rm     = BITS(inst,  0,  3);
-        if (CHECK_RM || CHECK_RN) 
-                inst_base->load_r15 = 1;
+    inst_cream->Rn     = BITS(inst, 16, 19);
+    inst_cream->Rd     = BITS(inst, 12, 15);
+    inst_cream->rotate = BITS(inst, 10, 11);
+    inst_cream->Rm     = BITS(inst,  0,  3);
+    if (CHECK_RM || CHECK_RN) 
+        inst_base->load_r15 = 1;
 
-        return inst_base;
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(ldrb)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
-        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
+    ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
 
-        inst_base->cond = BITS(inst, 28, 31);
-        inst_base->idx   = index;
-        inst_base->br    = NON_BRANCH;
+    inst_base->cond = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
 
-        inst_cream->inst = inst;
-        inst_cream->get_addr = get_calc_addr_op(inst);
+    inst_cream->inst = inst;
+    inst_cream->get_addr = get_calc_addr_op(inst);
 
-        if (BITS(inst, 12, 15) == 15) {
-                inst_base->br = INDIRECT_BRANCH;
-        }
-        return inst_base;
+    if (BITS(inst, 12, 15) == 15) {
+        inst_base->br = INDIRECT_BRANCH;
+    }
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(ldrbt)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
-        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
-
-        inst_base->cond = BITS(inst, 28, 31);
-        inst_base->idx   = index;
-        inst_base->br    = NON_BRANCH;
-
-        inst_cream->inst = inst;
-        if (I_BIT == 0) {
-                inst_cream->get_addr = LnSWoUB(ImmediatePostIndexed);
-        } else {
-                DEBUG_MSG;
-        }
-        #if 0
-        inst_cream->get_addr = get_calc_addr_op(inst);
-        if(inst == 0x54f13001) {
-                DEBUG_LOG(ARM11, "get_calc_addr_op:%llx\n", inst_cream->get_addr);
-        }
-        #endif
-
-        if (BITS(inst, 12, 15) == 15) {
-                inst_base->br = INDIRECT_BRANCH;
-        }
-        return inst_base;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
+    ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+
+    inst_base->cond = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
+
+    inst_cream->inst = inst;
+    if (I_BIT == 0) {
+        inst_cream->get_addr = LnSWoUB(ImmediatePostIndexed);
+    } else {
+        DEBUG_MSG;
+    }
+    #if 0
+    inst_cream->get_addr = get_calc_addr_op(inst);
+    if(inst == 0x54f13001) {
+        DEBUG_LOG(ARM11, "get_calc_addr_op:%llx\n", inst_cream->get_addr);
+    }
+    #endif
+
+    if (BITS(inst, 12, 15) == 15) {
+        inst_base->br = INDIRECT_BRANCH;
+    }
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(ldrd)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
-        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
+    ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
 
-        inst_base->cond = BITS(inst, 28, 31);
-        inst_base->idx   = index;
-        inst_base->br    = NON_BRANCH;
+    inst_base->cond = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
 
-        inst_cream->inst = inst;
-        inst_cream->get_addr = get_calc_addr_op(inst);
+    inst_cream->inst = inst;
+    inst_cream->get_addr = get_calc_addr_op(inst);
 
-        return inst_base;
+    return inst_base;
 }
 
 ARM_INST_PTR INTERPRETER_TRANSLATE(ldrex)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
-        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
+    ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
 
-        inst_base->cond = BITS(inst, 28, 31);
-        inst_base->idx   = index;
-        inst_base->br    = NON_BRANCH;
+    inst_base->cond = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
 
-        inst_cream->inst = inst;
-        //inst_cream->get_addr = get_calc_addr_op(inst);
+    inst_cream->inst = inst;
+    //inst_cream->get_addr = get_calc_addr_op(inst);
 
-        if (BITS(inst, 12, 15) == 15) {
-                inst_base->br = INDIRECT_BRANCH;
-        }
-        return inst_base;
+    if (BITS(inst, 12, 15) == 15) {
+        inst_base->br = INDIRECT_BRANCH;
+    }
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(ldrexb)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
-        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
+    ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
 
-        inst_base->cond = BITS(inst, 28, 31);
-        inst_base->idx   = index;
-        inst_base->br    = NON_BRANCH;
+    inst_base->cond = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
 
-        inst_cream->inst = inst;
-        inst_cream->get_addr = get_calc_addr_op(inst);
+    inst_cream->inst = inst;
+    inst_cream->get_addr = get_calc_addr_op(inst);
 
-        if (BITS(inst, 12, 15) == 15) {
-                inst_base->br = INDIRECT_BRANCH;
-        }
-        return inst_base;
+    if (BITS(inst, 12, 15) == 15) {
+        inst_base->br = INDIRECT_BRANCH;
+    }
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(ldrh)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
-        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
+    ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
 
-        inst_base->cond = BITS(inst, 28, 31);
-        inst_base->idx   = index;
-        inst_base->br    = NON_BRANCH;
+    inst_base->cond = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
 
-        inst_cream->inst = inst;
-        inst_cream->get_addr = get_calc_addr_op(inst);
+    inst_cream->inst = inst;
+    inst_cream->get_addr = get_calc_addr_op(inst);
 
-        if (BITS(inst, 12, 15) == 15) {
-                inst_base->br = INDIRECT_BRANCH;
-        }
-        return inst_base;
+    if (BITS(inst, 12, 15) == 15) {
+        inst_base->br = INDIRECT_BRANCH;
+    }
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(ldrsb)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
-        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
+    ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
 
-        inst_base->cond = BITS(inst, 28, 31);
-        inst_base->idx   = index;
-        inst_base->br    = NON_BRANCH;
+    inst_base->cond = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
 
-        inst_cream->inst = inst;
-        inst_cream->get_addr = get_calc_addr_op(inst);
+    inst_cream->inst = inst;
+    inst_cream->get_addr = get_calc_addr_op(inst);
 
-        if (BITS(inst, 12, 15) == 15) {
-                inst_base->br = INDIRECT_BRANCH;
-        }
-        return inst_base;
+    if (BITS(inst, 12, 15) == 15) {
+        inst_base->br = INDIRECT_BRANCH;
+    }
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(ldrsh)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
-        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
+    ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
 
-        inst_base->cond = BITS(inst, 28, 31);
-        inst_base->idx   = index;
-        inst_base->br    = NON_BRANCH;
+    inst_base->cond = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
 
-        inst_cream->inst = inst;
-        inst_cream->get_addr = get_calc_addr_op(inst);
+    inst_cream->inst = inst;
+    inst_cream->get_addr = get_calc_addr_op(inst);
 
-        if (BITS(inst, 12, 15) == 15) {
-                inst_base->br = INDIRECT_BRANCH;
-        }
-        return inst_base;
+    if (BITS(inst, 12, 15) == 15) {
+        inst_base->br = INDIRECT_BRANCH;
+    }
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(ldrt)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
-        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
-
-        inst_base->cond = BITS(inst, 28, 31);
-        inst_base->idx   = index;
-        inst_base->br    = NON_BRANCH;
-
-        inst_cream->inst = inst;
-        if (I_BIT == 0) {
-                inst_cream->get_addr = LnSWoUB(ImmediatePostIndexed);
-        } else {
-                DEBUG_MSG;
-        }
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
+    ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+
+    inst_base->cond = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
+
+    inst_cream->inst = inst;
+    if (I_BIT == 0) {
+        inst_cream->get_addr = LnSWoUB(ImmediatePostIndexed);
+    } else {
+        DEBUG_MSG;
+    }
 
-        if (BITS(inst, 12, 15) == 15) {
-                inst_base->br = INDIRECT_BRANCH;
-        }
-        return inst_base;
+    if (BITS(inst, 12, 15) == 15) {
+        inst_base->br = INDIRECT_BRANCH;
+    }
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(mcr)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(mcr_inst));
-        mcr_inst *inst_cream = (mcr_inst *)inst_base->component;
-        inst_base->cond  = BITS(inst, 28, 31);
-        inst_base->idx   = index;
-        inst_base->br    = NON_BRANCH;
-
-        inst_cream->crn      = BITS(inst, 16, 19);
-        inst_cream->crm      = BITS(inst,  0,  3);
-        inst_cream->opcode_1 = BITS(inst, 21, 23);
-        inst_cream->opcode_2 = BITS(inst,  5,  7);
-        inst_cream->Rd       = BITS(inst, 12, 15);
-        inst_cream->cp_num   = BITS(inst,  8, 11);
-        inst_cream->inst     = inst;
-        return inst_base;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(mcr_inst));
+    mcr_inst *inst_cream = (mcr_inst *)inst_base->component;
+    inst_base->cond  = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
+
+    inst_cream->crn      = BITS(inst, 16, 19);
+    inst_cream->crm      = BITS(inst,  0,  3);
+    inst_cream->opcode_1 = BITS(inst, 21, 23);
+    inst_cream->opcode_2 = BITS(inst,  5,  7);
+    inst_cream->Rd       = BITS(inst, 12, 15);
+    inst_cream->cp_num   = BITS(inst,  8, 11);
+    inst_cream->inst     = inst;
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(mcrr)(unsigned int inst, int index) { UNIMPLEMENTED_INSTRUCTION("MCRR"); }
 ARM_INST_PTR INTERPRETER_TRANSLATE(mla)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(mla_inst));
-        mla_inst *inst_cream = (mla_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(mla_inst));
+    mla_inst *inst_cream = (mla_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_base->cond  = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
+    inst_base->load_r15 = 0;
 
-        inst_cream->S    = BIT(inst, 20);
-        inst_cream->Rn   = BITS(inst, 12, 15);
-        inst_cream->Rd   = BITS(inst, 16, 19);
-        inst_cream->Rs   = BITS(inst,  8, 11);
-        inst_cream->Rm   = BITS(inst,  0,  3);
+    inst_cream->S     = BIT(inst, 20);
+    inst_cream->Rn     = BITS(inst, 12, 15);
+    inst_cream->Rd     = BITS(inst, 16, 19);
+    inst_cream->Rs     = BITS(inst,  8, 11);
+    inst_cream->Rm     = BITS(inst,  0,  3);
 
-        if (CHECK_RM || CHECK_RN || CHECK_RS) 
-                inst_base->load_r15 = 1;
+    if (CHECK_RM || CHECK_RN || CHECK_RS) 
+        inst_base->load_r15 = 1;
 
-        return inst_base;
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(mov)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(mov_inst));
-        mov_inst *inst_cream = (mov_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(mov_inst));
+    mov_inst *inst_cream = (mov_inst *)inst_base->component;
 
-        inst_base->cond  = BITS(inst, 28, 31);
-        inst_base->idx   = index;
-        inst_base->br    = NON_BRANCH;
+    inst_base->cond  = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
 
-        inst_cream->I    = BIT(inst, 25);
-        inst_cream->S    = BIT(inst, 20);
-        inst_cream->Rd   = BITS(inst, 12, 15);
-        inst_cream->shifter_operand = BITS(inst, 0, 11);
-        inst_cream->shtop_func = get_shtop(inst);
+    inst_cream->I     = BIT(inst, 25);
+    inst_cream->S     = BIT(inst, 20);
+    inst_cream->Rd     = BITS(inst, 12, 15);
+    inst_cream->shifter_operand = BITS(inst, 0, 11);
+    inst_cream->shtop_func = get_shtop(inst);
 
-        if (inst_cream->Rd == 15) {
-                inst_base->br = INDIRECT_BRANCH;
-        }
-        return inst_base;
+    if (inst_cream->Rd == 15) {
+        inst_base->br = INDIRECT_BRANCH;
+    }
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(mrc)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(mrc_inst));
-        mrc_inst *inst_cream = (mrc_inst *)inst_base->component;
-        inst_base->cond  = BITS(inst, 28, 31);
-        inst_base->idx   = index;
-        inst_base->br    = NON_BRANCH;
-
-        inst_cream->crn      = BITS(inst, 16, 19);
-        inst_cream->crm      = BITS(inst,  0,  3);
-        inst_cream->opcode_1 = BITS(inst, 21, 23);
-        inst_cream->opcode_2 = BITS(inst,  5,  7);
-        inst_cream->Rd       = BITS(inst, 12, 15);
-        inst_cream->cp_num   = BITS(inst,  8, 11);
-        inst_cream->inst     = inst;
-        return inst_base;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(mrc_inst));
+    mrc_inst *inst_cream = (mrc_inst *)inst_base->component;
+    inst_base->cond  = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
+
+    inst_cream->crn      = BITS(inst, 16, 19);
+    inst_cream->crm      = BITS(inst,  0,  3);
+    inst_cream->opcode_1 = BITS(inst, 21, 23);
+    inst_cream->opcode_2 = BITS(inst,  5,  7);
+    inst_cream->Rd       = BITS(inst, 12, 15);
+    inst_cream->cp_num   = BITS(inst,  8, 11);
+    inst_cream->inst     = inst;
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(mrrc)(unsigned int inst, int index) { UNIMPLEMENTED_INSTRUCTION("MRRC"); }
 ARM_INST_PTR INTERPRETER_TRANSLATE(mrs)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(mrs_inst));
-        mrs_inst *inst_cream = (mrs_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(mrs_inst));
+    mrs_inst *inst_cream = (mrs_inst *)inst_base->component;
 
-        inst_base->cond  = BITS(inst, 28, 31);
-        inst_base->idx   = index;
-        inst_base->br    = NON_BRANCH;
+    inst_base->cond  = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
 
-        inst_cream->Rd   = BITS(inst, 12, 15);
-        inst_cream->R    = BIT(inst, 22);
+    inst_cream->Rd   = BITS(inst, 12, 15);
+    inst_cream->R    = BIT(inst, 22);
 
-        return inst_base;
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(msr)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(msr_inst));
-        msr_inst *inst_cream = (msr_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(msr_inst));
+    msr_inst *inst_cream = (msr_inst *)inst_base->component;
 
-        inst_base->cond  = BITS(inst, 28, 31);
-        inst_base->idx   = index;
-        inst_base->br    = NON_BRANCH;
+    inst_base->cond  = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
 
-        inst_cream->field_mask = BITS(inst, 16, 19);
-        inst_cream->R          = BIT(inst, 22);
-        inst_cream->inst       = inst;
+    inst_cream->field_mask = BITS(inst, 16, 19);
+    inst_cream->R          = BIT(inst, 22);
+    inst_cream->inst       = inst;
 
-        return inst_base;
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(mul)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(mul_inst));
-        mul_inst *inst_cream = (mul_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(mul_inst));
+    mul_inst *inst_cream = (mul_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_base->cond  = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
+    inst_base->load_r15 = 0;
 
-        inst_cream->S    = BIT(inst, 20);
-        inst_cream->Rm   = BITS(inst, 0, 3);
-        inst_cream->Rs   = BITS(inst, 8, 11);
-        inst_cream->Rd   = BITS(inst, 16, 19);
+    inst_cream->S     = BIT(inst, 20);
+    inst_cream->Rm     = BITS(inst, 0, 3);
+    inst_cream->Rs     = BITS(inst, 8, 11);
+    inst_cream->Rd     = BITS(inst, 16, 19);
 
-        if (CHECK_RM || CHECK_RS) 
-                inst_base->load_r15 = 1;
-        return inst_base;
+    if (CHECK_RM || CHECK_RS) 
+        inst_base->load_r15 = 1;
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(mvn)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(mvn_inst));
-        mvn_inst *inst_cream = (mvn_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(mvn_inst));
+    mvn_inst *inst_cream = (mvn_inst *)inst_base->component;
 
-        inst_base->cond  = BITS(inst, 28, 31);
-        inst_base->idx   = index;
-        inst_base->br    = NON_BRANCH;
+    inst_base->cond  = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
 
-        inst_cream->I    = BIT(inst, 25);
-        inst_cream->S    = BIT(inst, 20);
-        inst_cream->Rd   = BITS(inst, 12, 15);
-        inst_cream->shifter_operand = BITS(inst, 0, 11);
-        inst_cream->shtop_func = get_shtop(inst);
+    inst_cream->I     = BIT(inst, 25);
+    inst_cream->S     = BIT(inst, 20);
+    inst_cream->Rd     = BITS(inst, 12, 15);
+    inst_cream->shifter_operand = BITS(inst, 0, 11);
+    inst_cream->shtop_func = get_shtop(inst);
 
-        if (inst_cream->Rd == 15) {
-                inst_base->br = INDIRECT_BRANCH;
-        }
-        return inst_base;
+    if (inst_cream->Rd == 15) {
+        inst_base->br = INDIRECT_BRANCH;
+    }
+    return inst_base;
 
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(orr)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(orr_inst));
-        orr_inst *inst_cream = (orr_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->I    = BIT(inst, 25);
-        inst_cream->S    = BIT(inst, 20);
-        inst_cream->Rd   = BITS(inst, 12, 15);
-        inst_cream->Rn   = BITS(inst, 16, 19);
-        inst_cream->shifter_operand = BITS(inst, 0, 11);
-        inst_cream->shtop_func = get_shtop(inst);
-
-        if (CHECK_RN) 
-                inst_base->load_r15 = 1;
-        if (inst_cream->Rd == 15) {
-                inst_base->br = INDIRECT_BRANCH;
-        }
-        return inst_base;
-}
-ARM_INST_PTR INTERPRETER_TRANSLATE(pkhbt)(unsigned int inst, int index) { UNIMPLEMENTED_INSTRUCTION("PKHBT"); }
-ARM_INST_PTR INTERPRETER_TRANSLATE(pkhtb)(unsigned int inst, int index) { UNIMPLEMENTED_INSTRUCTION("PKHTB"); }
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(orr_inst));
+    orr_inst *inst_cream = (orr_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->I     = BIT(inst, 25);
+    inst_cream->S     = BIT(inst, 20);
+    inst_cream->Rd     = BITS(inst, 12, 15);
+    inst_cream->Rn     = BITS(inst, 16, 19);
+    inst_cream->shifter_operand = BITS(inst, 0, 11);
+    inst_cream->shtop_func = get_shtop(inst);
+
+    if (CHECK_RN) 
+        inst_base->load_r15 = 1;
+    if (inst_cream->Rd == 15) {
+        inst_base->br = INDIRECT_BRANCH;
+    }
+    return inst_base;
+}
+
+ARM_INST_PTR INTERPRETER_TRANSLATE(pkhbt)(unsigned int inst, int index)
+{
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(pkh_inst));
+    pkh_inst *inst_cream = (pkh_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->Rd = BITS(inst, 12, 15);
+    inst_cream->Rn = BITS(inst, 16, 19);
+    inst_cream->Rm = BITS(inst, 0, 3);
+    inst_cream->imm = BITS(inst, 7, 11);
+
+    return inst_base;
+}
+
+ARM_INST_PTR INTERPRETER_TRANSLATE(pkhtb)(unsigned int inst, int index)
+{
+    return INTERPRETER_TRANSLATE(pkhbt)(inst, index);
+}
+
 ARM_INST_PTR INTERPRETER_TRANSLATE(pld)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(pld_inst));
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(pld_inst));
 
-        inst_base->cond  = BITS(inst, 28, 31);
-        inst_base->idx   = index;
-        inst_base->br    = NON_BRANCH;
-        inst_base->load_r15 = 0;
+    inst_base->cond  = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
+    inst_base->load_r15 = 0;
 
-        return inst_base;
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(qadd)(unsigned int inst, int index)     { UNIMPLEMENTED_INSTRUCTION("QADD"); }
-ARM_INST_PTR INTERPRETER_TRANSLATE(qadd16)(unsigned int inst, int index)   { UNIMPLEMENTED_INSTRUCTION("QADD16"); }
-ARM_INST_PTR INTERPRETER_TRANSLATE(qadd8)(unsigned int inst, int index)    { UNIMPLEMENTED_INSTRUCTION("QADD8"); }
-ARM_INST_PTR INTERPRETER_TRANSLATE(qaddsubx)(unsigned int inst, int index) { UNIMPLEMENTED_INSTRUCTION("QADDSUBX"); }
+ARM_INST_PTR INTERPRETER_TRANSLATE(qadd8)(unsigned int inst, int index)
+{
+    arm_inst* const inst_base = (arm_inst*)AllocBuffer(sizeof(arm_inst) + sizeof(generic_arm_inst));
+    generic_arm_inst* const inst_cream = (generic_arm_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, 0, 3);
+    inst_cream->Rn  = BITS(inst, 16, 19);
+    inst_cream->Rd  = BITS(inst, 12, 15);
+    inst_cream->op1 = BITS(inst, 20, 21);
+    inst_cream->op2 = BITS(inst, 5, 7);
+
+    return inst_base;
+}
+ARM_INST_PTR INTERPRETER_TRANSLATE(qadd16)(unsigned int inst, int index)
+{
+    return INTERPRETER_TRANSLATE(qadd8)(inst, index);
+}
+ARM_INST_PTR INTERPRETER_TRANSLATE(qaddsubx)(unsigned int inst, int index)
+{
+    return INTERPRETER_TRANSLATE(qadd8)(inst, index);
+}
 ARM_INST_PTR INTERPRETER_TRANSLATE(qdadd)(unsigned int inst, int index)    { UNIMPLEMENTED_INSTRUCTION("QDADD"); }
 ARM_INST_PTR INTERPRETER_TRANSLATE(qdsub)(unsigned int inst, int index)    { UNIMPLEMENTED_INSTRUCTION("QDSUB"); }
 ARM_INST_PTR INTERPRETER_TRANSLATE(qsub)(unsigned int inst, int index)     { UNIMPLEMENTED_INSTRUCTION("QSUB"); }
-ARM_INST_PTR INTERPRETER_TRANSLATE(qsub16)(unsigned int inst, int index)   { UNIMPLEMENTED_INSTRUCTION("QSUB16"); }
-ARM_INST_PTR INTERPRETER_TRANSLATE(qsub8)(unsigned int inst, int index)    { UNIMPLEMENTED_INSTRUCTION("QSUB8"); }
-ARM_INST_PTR INTERPRETER_TRANSLATE(qsubaddx)(unsigned int inst, int index) { UNIMPLEMENTED_INSTRUCTION("QSUBADDX"); }
+ARM_INST_PTR INTERPRETER_TRANSLATE(qsub8)(unsigned int inst, int index)
+{
+    return INTERPRETER_TRANSLATE(qadd8)(inst, index);
+}
+ARM_INST_PTR INTERPRETER_TRANSLATE(qsub16)(unsigned int inst, int index)
+{
+    return INTERPRETER_TRANSLATE(qadd8)(inst, index);
+}
+ARM_INST_PTR INTERPRETER_TRANSLATE(qsubaddx)(unsigned int inst, int index)
+{
+    return INTERPRETER_TRANSLATE(qadd8)(inst, index);
+}
 ARM_INST_PTR INTERPRETER_TRANSLATE(rev)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(rev_inst));
-        rev_inst *inst_cream = (rev_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(rev_inst));
+    rev_inst *inst_cream = (rev_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_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,  0,  3);
-        inst_cream->Rd   = BITS(inst, 12, 15);
+    inst_cream->Rm   = BITS(inst,  0,  3);
+    inst_cream->Rd   = BITS(inst, 12, 15);
 
-        return inst_base;
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(rev16)(unsigned int inst, int index){
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(rev_inst));
-        rev_inst *inst_cream = (rev_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(rev_inst));
+    rev_inst *inst_cream = (rev_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_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,  0,  3);
-        inst_cream->Rd   = BITS(inst, 12, 15);
+    inst_cream->Rm   = BITS(inst,  0,  3);
+    inst_cream->Rd   = BITS(inst, 12, 15);
 
-        return inst_base;
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(revsh)(unsigned int inst, int index) { UNIMPLEMENTED_INSTRUCTION("REVSH"); }
 ARM_INST_PTR INTERPRETER_TRANSLATE(rfe)(unsigned int inst, int index)   { UNIMPLEMENTED_INSTRUCTION("RFE"); }
 ARM_INST_PTR INTERPRETER_TRANSLATE(rsb)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(rsb_inst));
-        rsb_inst *inst_cream = (rsb_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->I    = BIT(inst, 25);
-        inst_cream->S    = BIT(inst, 20);
-        inst_cream->Rn   = BITS(inst, 16, 19);
-        inst_cream->Rd   = BITS(inst, 12, 15);
-        inst_cream->shifter_operand = BITS(inst, 0, 11);
-        inst_cream->shtop_func = get_shtop(inst);
-        if (CHECK_RN) 
-                inst_base->load_r15 = 1;
-
-        if (inst_cream->Rd == 15) {
-                inst_base->br = INDIRECT_BRANCH;
-        }
-        return inst_base;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(rsb_inst));
+    rsb_inst *inst_cream = (rsb_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->I     = BIT(inst, 25);
+    inst_cream->S     = BIT(inst, 20);
+    inst_cream->Rn     = BITS(inst, 16, 19);
+    inst_cream->Rd     = BITS(inst, 12, 15);
+    inst_cream->shifter_operand = BITS(inst, 0, 11);
+    inst_cream->shtop_func = get_shtop(inst);
+    if (CHECK_RN) 
+        inst_base->load_r15 = 1;
+
+    if (inst_cream->Rd == 15) {
+        inst_base->br = INDIRECT_BRANCH;
+    }
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(rsc)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(rsc_inst));
-        rsc_inst *inst_cream = (rsc_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->I    = BIT(inst, 25);
-        inst_cream->S    = BIT(inst, 20);
-        inst_cream->Rn   = BITS(inst, 16, 19);
-        inst_cream->Rd   = BITS(inst, 12, 15);
-        inst_cream->shifter_operand = BITS(inst, 0, 11);
-        inst_cream->shtop_func = get_shtop(inst);
-        if (CHECK_RN)
-                inst_base->load_r15 = 1;
-
-        if (inst_cream->Rd == 15) {
-                inst_base->br = INDIRECT_BRANCH;
-        }
-        return inst_base;
-}
-ARM_INST_PTR INTERPRETER_TRANSLATE(sadd16)(unsigned int inst, int index)   { UNIMPLEMENTED_INSTRUCTION("SADD16"); }
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(rsc_inst));
+    rsc_inst *inst_cream = (rsc_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->I     = BIT(inst, 25);
+    inst_cream->S     = BIT(inst, 20);
+    inst_cream->Rn     = BITS(inst, 16, 19);
+    inst_cream->Rd     = BITS(inst, 12, 15);
+    inst_cream->shifter_operand = BITS(inst, 0, 11);
+    inst_cream->shtop_func = get_shtop(inst);
+    if (CHECK_RN)
+        inst_base->load_r15 = 1;
+
+    if (inst_cream->Rd == 15) {
+        inst_base->br = INDIRECT_BRANCH;
+    }
+    return inst_base;
+}
 ARM_INST_PTR INTERPRETER_TRANSLATE(sadd8)(unsigned int inst, int index)    { UNIMPLEMENTED_INSTRUCTION("SADD8"); }
-ARM_INST_PTR INTERPRETER_TRANSLATE(saddsubx)(unsigned int inst, int index) { UNIMPLEMENTED_INSTRUCTION("SADDSUBX"); }
+ARM_INST_PTR INTERPRETER_TRANSLATE(sadd16)(unsigned int inst, int index)
+{
+    arm_inst* const inst_base = (arm_inst*)AllocBuffer(sizeof(arm_inst) + sizeof(generic_arm_inst));
+    generic_arm_inst* const inst_cream = (generic_arm_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, 0, 3);
+    inst_cream->Rn  = BITS(inst, 16, 19);
+    inst_cream->Rd  = BITS(inst, 12, 15);
+    inst_cream->op1 = BITS(inst, 20, 21);
+    inst_cream->op2 = BITS(inst, 5, 7);
+    
+    return inst_base;
+}
+ARM_INST_PTR INTERPRETER_TRANSLATE(saddsubx)(unsigned int inst, int index)
+{
+    return INTERPRETER_TRANSLATE(sadd16)(inst, index);
+}
 ARM_INST_PTR INTERPRETER_TRANSLATE(sbc)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(sbc_inst));
-        sbc_inst *inst_cream = (sbc_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->I    = BIT(inst, 25);
-        inst_cream->S    = BIT(inst, 20);
-        inst_cream->Rn   = BITS(inst, 16, 19);
-        inst_cream->Rd   = BITS(inst, 12, 15);
-        inst_cream->shifter_operand = BITS(inst, 0, 11);
-        inst_cream->shtop_func = get_shtop(inst);
-        if (CHECK_RN)
-                inst_base->load_r15 = 1;
-
-        if (inst_cream->Rd == 15) {
-                inst_base->br = INDIRECT_BRANCH;
-        }
-        return inst_base;
-}
-ARM_INST_PTR INTERPRETER_TRANSLATE(sel)(unsigned int inst, int index)       { UNIMPLEMENTED_INSTRUCTION("SEL"); }
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(sbc_inst));
+    sbc_inst *inst_cream = (sbc_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->I     = BIT(inst, 25);
+    inst_cream->S     = BIT(inst, 20);
+    inst_cream->Rn     = BITS(inst, 16, 19);
+    inst_cream->Rd     = BITS(inst, 12, 15);
+    inst_cream->shifter_operand = BITS(inst, 0, 11);
+    inst_cream->shtop_func = get_shtop(inst);
+    if (CHECK_RN)
+        inst_base->load_r15 = 1;
+
+    if (inst_cream->Rd == 15) {
+        inst_base->br = INDIRECT_BRANCH;
+    }
+    return inst_base;
+}
+ARM_INST_PTR INTERPRETER_TRANSLATE(sel)(unsigned int inst, int index)
+{
+    arm_inst* const inst_base = (arm_inst*)AllocBuffer(sizeof(arm_inst) + sizeof(generic_arm_inst));
+    generic_arm_inst* const inst_cream = (generic_arm_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, 0, 3);
+    inst_cream->Rn  = BITS(inst, 16, 19);
+    inst_cream->Rd  = BITS(inst, 12, 15);
+    inst_cream->op1 = BITS(inst, 20, 22);
+    inst_cream->op2 = BITS(inst, 5, 7);
+
+    return inst_base;
+}
 ARM_INST_PTR INTERPRETER_TRANSLATE(setend)(unsigned int inst, int index)    { UNIMPLEMENTED_INSTRUCTION("SETEND"); }
 ARM_INST_PTR INTERPRETER_TRANSLATE(shadd16)(unsigned int inst, int index)   { UNIMPLEMENTED_INSTRUCTION("SHADD16"); }
 ARM_INST_PTR INTERPRETER_TRANSLATE(shadd8)(unsigned int inst, int index)    { UNIMPLEMENTED_INSTRUCTION("SHADD8"); }
@@ -2499,61 +2396,61 @@ ARM_INST_PTR INTERPRETER_TRANSLATE(shsub8)(unsigned int inst, int index)    { UN
 ARM_INST_PTR INTERPRETER_TRANSLATE(shsubaddx)(unsigned int inst, int index) { UNIMPLEMENTED_INSTRUCTION("SHSUBADDX"); }
 ARM_INST_PTR INTERPRETER_TRANSLATE(smla)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(smla_inst));
-        smla_inst *inst_cream = (smla_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(smla_inst));
+    smla_inst *inst_cream = (smla_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_base->cond  = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
+    inst_base->load_r15 = 0;
 
-        inst_cream->x    = BIT(inst, 5);
-        inst_cream->y    = BIT(inst, 6);
-        inst_cream->Rm   = BITS(inst, 0, 3);
-        inst_cream->Rs   = BITS(inst, 8, 11);
-        inst_cream->Rd = BITS(inst, 16, 19);
-        inst_cream->Rn = BITS(inst, 12, 15);
+    inst_cream->x     = BIT(inst, 5);
+    inst_cream->y     = BIT(inst, 6);
+    inst_cream->Rm     = BITS(inst, 0, 3);
+    inst_cream->Rs     = BITS(inst, 8, 11);
+    inst_cream->Rd = BITS(inst, 16, 19);
+    inst_cream->Rn = BITS(inst, 12, 15);
 
-        return inst_base;
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(smlad)(unsigned int inst, int index){
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(smlad_inst));
-        smlad_inst *inst_cream = (smlad_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(smlad_inst));
+    smlad_inst *inst_cream = (smlad_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_base->cond  = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
+    inst_base->load_r15 = 0;
 
-        inst_cream->m    = BIT(inst, 4);
-        inst_cream->Rn   = BITS(inst, 0, 3);
-        inst_cream->Rm   = BITS(inst, 8, 11);
-        inst_cream->Rd = BITS(inst, 16, 19);
-        inst_cream->Ra = BITS(inst, 12, 15);
+    inst_cream->m     = BIT(inst, 4);
+    inst_cream->Rn     = BITS(inst, 0, 3);
+    inst_cream->Rm     = BITS(inst, 8, 11);
+    inst_cream->Rd = BITS(inst, 16, 19);
+    inst_cream->Ra = BITS(inst, 12, 15);
 
-        if (CHECK_RM ) 
-                inst_base->load_r15 = 1;
-        return inst_base;
+    if (CHECK_RM ) 
+        inst_base->load_r15 = 1;
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(smlal)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(umlal_inst));
-        umlal_inst *inst_cream = (umlal_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(umlal_inst));
+    umlal_inst *inst_cream = (umlal_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_base->cond  = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
+    inst_base->load_r15 = 0;
 
-        inst_cream->S    = BIT(inst, 20);
-        inst_cream->Rm   = BITS(inst, 0, 3);
-        inst_cream->Rs   = BITS(inst, 8, 11);
-        inst_cream->RdHi = BITS(inst, 16, 19);
-        inst_cream->RdLo = BITS(inst, 12, 15);
+    inst_cream->S     = BIT(inst, 20);
+    inst_cream->Rm     = BITS(inst, 0, 3);
+    inst_cream->Rs     = BITS(inst, 8, 11);
+    inst_cream->RdHi = BITS(inst, 16, 19);
+    inst_cream->RdLo = BITS(inst, 12, 15);
 
-        if (CHECK_RM || CHECK_RS) 
-                inst_base->load_r15 = 1;
-        return inst_base;
+    if (CHECK_RM || CHECK_RS) 
+        inst_base->load_r15 = 1;
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(smlalxy)(unsigned int inst, int index) { UNIMPLEMENTED_INSTRUCTION("SMLALXY"); }
 ARM_INST_PTR INTERPRETER_TRANSLATE(smlald)(unsigned int inst, int index)  { UNIMPLEMENTED_INSTRUCTION("SMLALD"); }
@@ -2566,572 +2463,700 @@ ARM_INST_PTR INTERPRETER_TRANSLATE(smmul)(unsigned int inst, int index)   { UNIM
 ARM_INST_PTR INTERPRETER_TRANSLATE(smuad)(unsigned int inst, int index)   { UNIMPLEMENTED_INSTRUCTION("SMUAD"); }
 ARM_INST_PTR INTERPRETER_TRANSLATE(smul)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(smul_inst));
-        smul_inst *inst_cream = (smul_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(smul_inst));
+    smul_inst *inst_cream = (smul_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_base->cond  = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
+    inst_base->load_r15 = 0;
 
-        inst_cream->Rd = BITS(inst, 16, 19);
-        inst_cream->Rs = BITS(inst,  8, 11);
-        inst_cream->Rm = BITS(inst,  0,  3);
+    inst_cream->Rd = BITS(inst, 16, 19);
+    inst_cream->Rs = BITS(inst,  8, 11);
+    inst_cream->Rm = BITS(inst,  0,  3);
 
-        inst_cream->x  = BIT(inst, 5);
-        inst_cream->y  = BIT(inst, 6);
+    inst_cream->x  = BIT(inst, 5);
+    inst_cream->y  = BIT(inst, 6);
 
-        if (CHECK_RM || CHECK_RS) 
-                inst_base->load_r15 = 1;
-        return inst_base;
+    if (CHECK_RM || CHECK_RS) 
+        inst_base->load_r15 = 1;
+    return inst_base;
 
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(smull)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(umull_inst));
-        umull_inst *inst_cream = (umull_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(umull_inst));
+    umull_inst *inst_cream = (umull_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_base->cond  = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
+    inst_base->load_r15 = 0;
 
-        inst_cream->S    = BIT(inst, 20);
-        inst_cream->Rm   = BITS(inst, 0, 3);
-        inst_cream->Rs   = BITS(inst, 8, 11);
-        inst_cream->RdHi = BITS(inst, 16, 19);
-        inst_cream->RdLo = BITS(inst, 12, 15);
+    inst_cream->S     = BIT(inst, 20);
+    inst_cream->Rm     = BITS(inst, 0, 3);
+    inst_cream->Rs     = BITS(inst, 8, 11);
+    inst_cream->RdHi = BITS(inst, 16, 19);
+    inst_cream->RdLo = BITS(inst, 12, 15);
 
-        if (CHECK_RM || CHECK_RS) 
-                inst_base->load_r15 = 1;
-        return inst_base;
+    if (CHECK_RM || CHECK_RS) 
+        inst_base->load_r15 = 1;
+    return inst_base;
 }
 
 ARM_INST_PTR INTERPRETER_TRANSLATE(smulw)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(smlad_inst));
-        smlad_inst *inst_cream = (smlad_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(smlad_inst));
+    smlad_inst *inst_cream = (smlad_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_base->cond  = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
+    inst_base->load_r15 = 0;
 
-        inst_cream->m    = BIT(inst, 6);
-        inst_cream->Rm   = BITS(inst, 8, 11);
-        inst_cream->Rn   = BITS(inst, 0, 3);
-        inst_cream->Rd = BITS(inst, 16, 19);
+    inst_cream->m     = BIT(inst, 6);
+    inst_cream->Rm     = BITS(inst, 8, 11);
+    inst_cream->Rn     = BITS(inst, 0, 3);
+    inst_cream->Rd = BITS(inst, 16, 19);
 
-        if (CHECK_RM || CHECK_RN) 
-                inst_base->load_r15 = 1;
-        return inst_base;
+    if (CHECK_RM || CHECK_RN) 
+        inst_base->load_r15 = 1;
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(smusd)(unsigned int inst, int index)    { UNIMPLEMENTED_INSTRUCTION("SMUSD"); }
 ARM_INST_PTR INTERPRETER_TRANSLATE(srs)(unsigned int inst, int index)      { UNIMPLEMENTED_INSTRUCTION("SRS"); }
 ARM_INST_PTR INTERPRETER_TRANSLATE(ssat)(unsigned int inst, int index)     { UNIMPLEMENTED_INSTRUCTION("SSAT"); }
 ARM_INST_PTR INTERPRETER_TRANSLATE(ssat16)(unsigned int inst, int index)   { UNIMPLEMENTED_INSTRUCTION("SSAT16"); }
-ARM_INST_PTR INTERPRETER_TRANSLATE(ssub16)(unsigned int inst, int index)   { UNIMPLEMENTED_INSTRUCTION("SSUB16"); }
 ARM_INST_PTR INTERPRETER_TRANSLATE(ssub8)(unsigned int inst, int index)    { UNIMPLEMENTED_INSTRUCTION("SSUB8"); }
-ARM_INST_PTR INTERPRETER_TRANSLATE(ssubaddx)(unsigned int inst, int index) { UNIMPLEMENTED_INSTRUCTION("SSUBADDX"); }
+ARM_INST_PTR INTERPRETER_TRANSLATE(ssub16)(unsigned int inst, int index)
+{
+    return INTERPRETER_TRANSLATE(sadd16)(inst, index);
+}
+ARM_INST_PTR INTERPRETER_TRANSLATE(ssubaddx)(unsigned int inst, int index)
+{
+    return INTERPRETER_TRANSLATE(sadd16)(inst, index);
+}
 ARM_INST_PTR INTERPRETER_TRANSLATE(stc)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(stc_inst));
-        inst_base->cond  = BITS(inst, 28, 31);
-        inst_base->idx   = index;
-        inst_base->br    = NON_BRANCH;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(stc_inst));
+    inst_base->cond  = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
 
-        return inst_base;
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(stm)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
-        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
+    ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
 
-        inst_base->cond = BITS(inst, 28, 31);
-        inst_base->idx   = index;
-        inst_base->br    = NON_BRANCH;
+    inst_base->cond = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
 
-        inst_cream->inst = inst;
-        inst_cream->get_addr = get_calc_addr_op(inst);
-        return inst_base;
+    inst_cream->inst = inst;
+    inst_cream->get_addr = get_calc_addr_op(inst);
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(sxtb)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(sxtb_inst));
-        sxtb_inst *inst_cream = (sxtb_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(sxtb_inst));
+    sxtb_inst *inst_cream = (sxtb_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_base->cond  = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
+    inst_base->load_r15 = 0;
 
-        inst_cream->Rd     = BITS(inst, 12, 15);
-        inst_cream->Rm     = BITS(inst,  0,  3);
-        inst_cream->rotate = BITS(inst, 10, 11);
+    inst_cream->Rd     = BITS(inst, 12, 15);
+    inst_cream->Rm     = BITS(inst,  0,  3);
+    inst_cream->rotate = BITS(inst, 10, 11);
 
-        if (CHECK_RM) 
-                inst_base->load_r15 = 1;
-        return inst_base;
+    if (CHECK_RM) 
+        inst_base->load_r15 = 1;
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(str)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
-        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
+    ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
 
-        inst_base->cond = BITS(inst, 28, 31);
-        inst_base->idx   = index;
-        inst_base->br    = NON_BRANCH;
+    inst_base->cond = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
 
-        inst_cream->inst = inst;
-        inst_cream->get_addr = get_calc_addr_op(inst);
+    inst_cream->inst = inst;
+    inst_cream->get_addr = get_calc_addr_op(inst);
 
-        if (BITS(inst, 12, 15) == 15) {
-                inst_base->br = INDIRECT_BRANCH;
-        }
-        return inst_base;
+    if (BITS(inst, 12, 15) == 15) {
+        inst_base->br = INDIRECT_BRANCH;
+    }
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(uxtb)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(uxth_inst));
-        uxth_inst *inst_cream = (uxth_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(uxth_inst));
+    uxth_inst *inst_cream = (uxth_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_base->cond = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
+    inst_base->load_r15 = 0;
 
-        inst_cream->Rd     = BITS(inst, 12, 15);
-        inst_cream->rotate = BITS(inst, 10, 11);
-        inst_cream->Rm     = BITS(inst,  0,  3);
+    inst_cream->Rd     = BITS(inst, 12, 15);
+    inst_cream->rotate = BITS(inst, 10, 11);
+    inst_cream->Rm     = BITS(inst,  0,  3);
 
-        if (CHECK_RM) 
-                inst_base->load_r15 = 1;
-        return inst_base;
+    if (CHECK_RM) 
+        inst_base->load_r15 = 1;
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(uxtab)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(uxtab_inst));
-        uxtab_inst *inst_cream = (uxtab_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(uxtab_inst));
+    uxtab_inst *inst_cream = (uxtab_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_base->cond = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
+    inst_base->load_r15 = 0;
 
-        inst_cream->Rd     = BITS(inst, 12, 15);
-        inst_cream->rotate = BITS(inst, 10, 11);
-        inst_cream->Rm     = BITS(inst,  0,  3);
-        inst_cream->Rn     = BITS(inst, 16, 19);
+    inst_cream->Rd     = BITS(inst, 12, 15);
+    inst_cream->rotate = BITS(inst, 10, 11);
+    inst_cream->Rm     = BITS(inst,  0,  3);
+    inst_cream->Rn     = BITS(inst, 16, 19);
 
-        return inst_base;
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(strb)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
-        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
+    ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
 
-        inst_base->cond = BITS(inst, 28, 31);
-        inst_base->idx   = index;
-        inst_base->br    = NON_BRANCH;
+    inst_base->cond = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
 
-        inst_cream->inst = inst;
-        inst_cream->get_addr = get_calc_addr_op(inst);
+    inst_cream->inst = inst;
+    inst_cream->get_addr = get_calc_addr_op(inst);
 
-        if (BITS(inst, 12, 15) == 15) {
-                inst_base->br = INDIRECT_BRANCH;
-        }
-        return inst_base;
+    if (BITS(inst, 12, 15) == 15) {
+        inst_base->br = INDIRECT_BRANCH;
+    }
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(strbt)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
-        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
-
-        inst_base->cond = BITS(inst, 28, 31);
-        inst_base->idx   = index;
-        inst_base->br    = NON_BRANCH;
-
-        inst_cream->inst = inst;
-//      inst_cream->get_addr = get_calc_addr_op(inst);
-        if (I_BIT == 0) {
-                inst_cream->get_addr = LnSWoUB(ImmediatePostIndexed);
-        } else {
-                DEBUG_MSG;
-        }
-
-        if (BITS(inst, 12, 15) == 15) {
-                inst_base->br = INDIRECT_BRANCH;
-        }
-        return inst_base;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
+    ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+
+    inst_base->cond = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
+
+    inst_cream->inst = inst;
+//    inst_cream->get_addr = get_calc_addr_op(inst);
+    if (I_BIT == 0) {
+        inst_cream->get_addr = LnSWoUB(ImmediatePostIndexed);
+    } else {
+        DEBUG_MSG;
+    }
+
+    if (BITS(inst, 12, 15) == 15) {
+        inst_base->br = INDIRECT_BRANCH;
+    }
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(strd)(unsigned int inst, int index){
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
-        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
+    ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
 
-        inst_base->cond = BITS(inst, 28, 31);
-        inst_base->idx   = index;
-        inst_base->br    = NON_BRANCH;
+    inst_base->cond = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
 
-        inst_cream->inst = inst;
-        inst_cream->get_addr = get_calc_addr_op(inst);
+    inst_cream->inst = inst;
+    inst_cream->get_addr = get_calc_addr_op(inst);
 
-        if (BITS(inst, 12, 15) == 15) {
-                inst_base->br = INDIRECT_BRANCH;
-        }
-        return inst_base;
+    if (BITS(inst, 12, 15) == 15) {
+        inst_base->br = INDIRECT_BRANCH;
+    }
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(strex)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
-        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
+    ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
 
-        inst_base->cond = BITS(inst, 28, 31);
-        inst_base->idx   = index;
-        inst_base->br    = NON_BRANCH;
+    inst_base->cond = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
 
-        inst_cream->inst = inst;
-        inst_cream->get_addr = get_calc_addr_op(inst);
+    inst_cream->inst = inst;
+    inst_cream->get_addr = get_calc_addr_op(inst);
 
-        if (BITS(inst, 12, 15) == 15) {
-                inst_base->br = INDIRECT_BRANCH;
-        }
-        return inst_base;
+    if (BITS(inst, 12, 15) == 15) {
+        inst_base->br = INDIRECT_BRANCH;
+    }
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(strexb)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
-        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
+    ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
 
-        inst_base->cond = BITS(inst, 28, 31);
-        inst_base->idx   = index;
-        inst_base->br    = NON_BRANCH;
+    inst_base->cond = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
 
-        inst_cream->inst = inst;
-        inst_cream->get_addr = get_calc_addr_op(inst);
+    inst_cream->inst = inst;
+    inst_cream->get_addr = get_calc_addr_op(inst);
 
-        if (BITS(inst, 12, 15) == 15) {
-                inst_base->br = INDIRECT_BRANCH;
-        }
-        return inst_base;
+    if (BITS(inst, 12, 15) == 15) {
+        inst_base->br = INDIRECT_BRANCH;
+    }
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(strh)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
-        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
+    ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
 
-        inst_base->cond = BITS(inst, 28, 31);
-        inst_base->idx   = index;
-        inst_base->br    = NON_BRANCH;
+    inst_base->cond = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
 
-        inst_cream->inst = inst;
-        inst_cream->get_addr = get_calc_addr_op(inst);
+    inst_cream->inst = inst;
+    inst_cream->get_addr = get_calc_addr_op(inst);
 
-        if (BITS(inst, 12, 15) == 15) {
-                inst_base->br = INDIRECT_BRANCH;
-        }
-        return inst_base;
+    if (BITS(inst, 12, 15) == 15) {
+        inst_base->br = INDIRECT_BRANCH;
+    }
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(strt)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
-        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
-
-        inst_base->cond = BITS(inst, 28, 31);
-        inst_base->idx   = index;
-        inst_base->br    = NON_BRANCH;
-
-        inst_cream->inst = inst;
-        if (I_BIT == 0) {
-                inst_cream->get_addr = LnSWoUB(ImmediatePostIndexed);
-        } else {
-                DEBUG_MSG;
-        }
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(ldst_inst));
+    ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+
+    inst_base->cond = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
+
+    inst_cream->inst = inst;
+    if (I_BIT == 0) {
+        inst_cream->get_addr = LnSWoUB(ImmediatePostIndexed);
+    } else {
+        DEBUG_MSG;
+    }
 
-        if (BITS(inst, 12, 15) == 15) {
-                inst_base->br = INDIRECT_BRANCH;
-        }
-        return inst_base;
+    if (BITS(inst, 12, 15) == 15) {
+        inst_base->br = INDIRECT_BRANCH;
+    }
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(sub)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(sub_inst));
-        sub_inst *inst_cream = (sub_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->I    = BIT(inst, 25);
-        inst_cream->S    = BIT(inst, 20);
-        inst_cream->Rn   = BITS(inst, 16, 19);
-        inst_cream->Rd   = BITS(inst, 12, 15);
-        inst_cream->shifter_operand = BITS(inst, 0, 11);
-        inst_cream->shtop_func = get_shtop(inst);
-        if (inst_cream->Rd == 15) {
-                inst_base->br = INDIRECT_BRANCH;
-        }
-        if (CHECK_RN) 
-                inst_base->load_r15 = 1;
-
-        return inst_base;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(sub_inst));
+    sub_inst *inst_cream = (sub_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->I     = BIT(inst, 25);
+    inst_cream->S     = BIT(inst, 20);
+    inst_cream->Rn     = BITS(inst, 16, 19);
+    inst_cream->Rd     = BITS(inst, 12, 15);
+    inst_cream->shifter_operand = BITS(inst, 0, 11);
+    inst_cream->shtop_func = get_shtop(inst);
+    if (inst_cream->Rd == 15) {
+        inst_base->br = INDIRECT_BRANCH;
+    }
+    if (CHECK_RN) 
+        inst_base->load_r15 = 1;
+
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(swi)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(swi_inst));
-        swi_inst *inst_cream = (swi_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(swi_inst));
+    swi_inst *inst_cream = (swi_inst *)inst_base->component;
 
-        inst_base->cond  = BITS(inst, 28, 31);
-        inst_base->idx   = index;
-        inst_base->br    = NON_BRANCH;
+    inst_base->cond = BITS(inst, 28, 31);
+    inst_base->idx  = index;
+    inst_base->br   = NON_BRANCH;
 
-        inst_cream->num  = BITS(inst, 0, 23);
-        return inst_base;
+    inst_cream->num = BITS(inst, 0, 23);
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(swp)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(swp_inst));
-        swp_inst *inst_cream = (swp_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(swp_inst));
+    swp_inst *inst_cream = (swp_inst *)inst_base->component;
 
-        inst_base->cond  = BITS(inst, 28, 31);
-        inst_base->idx   = index;
-        inst_base->br    = NON_BRANCH;
+    inst_base->cond = BITS(inst, 28, 31);
+    inst_base->idx  = index;
+    inst_base->br   = NON_BRANCH;
 
-        inst_cream->Rn   = BITS(inst, 16, 19);
-        inst_cream->Rd   = BITS(inst, 12, 15);
-        inst_cream->Rm   = BITS(inst,  0,  3);
+    inst_cream->Rn  = BITS(inst, 16, 19);
+    inst_cream->Rd  = BITS(inst, 12, 15);
+    inst_cream->Rm  = BITS(inst,  0,  3);
 
-        if (inst_cream->Rd == 15) {
-                inst_base->br = INDIRECT_BRANCH;
-        }
-        return inst_base;
+    if (inst_cream->Rd == 15) {
+        inst_base->br = INDIRECT_BRANCH;
+    }
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(swpb)(unsigned int inst, int index){
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(swp_inst));
-        swp_inst *inst_cream = (swp_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(swp_inst));
+    swp_inst *inst_cream = (swp_inst *)inst_base->component;
 
-        inst_base->cond  = BITS(inst, 28, 31);
-        inst_base->idx   = index;
-        inst_base->br    = NON_BRANCH;
+    inst_base->cond = BITS(inst, 28, 31);
+    inst_base->idx  = index;
+    inst_base->br   = NON_BRANCH;
 
-        inst_cream->Rn   = BITS(inst, 16, 19);
-        inst_cream->Rd   = BITS(inst, 12, 15);
-        inst_cream->Rm   = BITS(inst,  0,  3);
+    inst_cream->Rn  = BITS(inst, 16, 19);
+    inst_cream->Rd  = BITS(inst, 12, 15);
+    inst_cream->Rm  = BITS(inst,  0,  3);
 
-        if (inst_cream->Rd == 15) {
-                inst_base->br = INDIRECT_BRANCH;
-        }
-        return inst_base;
+    if (inst_cream->Rd == 15) {
+        inst_base->br = INDIRECT_BRANCH;
+    }
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(sxtab)(unsigned int inst, int index){
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(sxtab_inst));
-        sxtab_inst *inst_cream = (sxtab_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(sxtab_inst));
+    sxtab_inst *inst_cream = (sxtab_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_base->cond     = BITS(inst, 28, 31);
+    inst_base->idx      = index;
+    inst_base->br       = NON_BRANCH;
+    inst_base->load_r15 = 0;
 
-        inst_cream->Rd     = BITS(inst, 12, 15);
-        inst_cream->rotate = BITS(inst, 10, 11);
-        inst_cream->Rm     = BITS(inst,  0,  3);
-        inst_cream->Rn     = BITS(inst, 16, 19);
+    inst_cream->Rd     = BITS(inst, 12, 15);
+    inst_cream->rotate = BITS(inst, 10, 11);
+    inst_cream->Rm     = BITS(inst,  0,  3);
+    inst_cream->Rn     = BITS(inst, 16, 19);
 
-        return inst_base;
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(sxtab16)(unsigned int inst, int index) { UNIMPLEMENTED_INSTRUCTION("SXTAB16"); }
 ARM_INST_PTR INTERPRETER_TRANSLATE(sxtah)(unsigned int inst, int index){
-        LOG_WARNING(Core_ARM11, "SXTAH untested");
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(sxtah_inst));
-        sxtah_inst *inst_cream = (sxtah_inst *)inst_base->component;
+    LOG_WARNING(Core_ARM11, "SXTAH untested");
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(sxtah_inst));
+    sxtah_inst *inst_cream = (sxtah_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_base->cond     = BITS(inst, 28, 31);
+    inst_base->idx      = index;
+    inst_base->br       = NON_BRANCH;
+    inst_base->load_r15 = 0;
 
-        inst_cream->Rd     = BITS(inst, 12, 15);
-        inst_cream->rotate = BITS(inst, 10, 11);
-        inst_cream->Rm     = BITS(inst,  0,  3);
-        inst_cream->Rn     = BITS(inst, 16, 19);
+    inst_cream->Rd     = BITS(inst, 12, 15);
+    inst_cream->rotate = BITS(inst, 10, 11);
+    inst_cream->Rm     = BITS(inst,  0,  3);
+    inst_cream->Rn     = BITS(inst, 16, 19);
 
-        return inst_base;
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(sxtb16)(unsigned int inst, int index) { UNIMPLEMENTED_INSTRUCTION("SXTB16"); }
 ARM_INST_PTR INTERPRETER_TRANSLATE(teq)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(teq_inst));
-        teq_inst *inst_cream = (teq_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(teq_inst));
+    teq_inst *inst_cream = (teq_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_base->cond     = BITS(inst, 28, 31);
+    inst_base->idx      = index;
+    inst_base->br       = NON_BRANCH;
+    inst_base->load_r15 = 0;
 
-        inst_cream->I    = BIT(inst, 25);
-        inst_cream->Rn   = BITS(inst, 16, 19);
-        inst_cream->shifter_operand = BITS(inst, 0, 11);
-        inst_cream->shtop_func = get_shtop(inst);
+    inst_cream->I               = BIT(inst, 25);
+    inst_cream->Rn              = BITS(inst, 16, 19);
+    inst_cream->shifter_operand = BITS(inst, 0, 11);
+    inst_cream->shtop_func      = get_shtop(inst);
 
-        if (CHECK_RN) 
-                inst_base->load_r15 = 1;
-        return inst_base;
+    if (CHECK_RN) 
+        inst_base->load_r15 = 1;
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(tst)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(tst_inst));
-        tst_inst *inst_cream = (tst_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->I    = BIT(inst, 25);
-        inst_cream->S    = BIT(inst, 20);
-        inst_cream->Rn   = BITS(inst, 16, 19);
-        inst_cream->Rd   = BITS(inst, 12, 15);
-        inst_cream->shifter_operand = BITS(inst, 0, 11);
-        inst_cream->shtop_func = get_shtop(inst);
-        if (inst_cream->Rd == 15) {
-                inst_base->br = INDIRECT_BRANCH;
-        }
-
-        if (CHECK_RN) 
-                inst_base->load_r15 = 1;
-        return inst_base;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(tst_inst));
+    tst_inst *inst_cream = (tst_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->I      = BIT(inst, 25);
+    inst_cream->S      = BIT(inst, 20);
+    inst_cream->Rn     = BITS(inst, 16, 19);
+    inst_cream->Rd     = BITS(inst, 12, 15);
+    inst_cream->shifter_operand = BITS(inst, 0, 11);
+    inst_cream->shtop_func = get_shtop(inst);
+    if (inst_cream->Rd == 15) {
+        inst_base->br = INDIRECT_BRANCH;
+    }
+
+    if (CHECK_RN) 
+        inst_base->load_r15 = 1;
+    return inst_base;
 }
-ARM_INST_PTR INTERPRETER_TRANSLATE(uadd16)(unsigned int inst, int index)    { UNIMPLEMENTED_INSTRUCTION("UADD16"); }
 ARM_INST_PTR INTERPRETER_TRANSLATE(uadd8)(unsigned int inst, int index)     { UNIMPLEMENTED_INSTRUCTION("UADD8"); }
+ARM_INST_PTR INTERPRETER_TRANSLATE(uadd16)(unsigned int inst, int index)    { UNIMPLEMENTED_INSTRUCTION("UADD16"); }
 ARM_INST_PTR INTERPRETER_TRANSLATE(uaddsubx)(unsigned int inst, int index)  { UNIMPLEMENTED_INSTRUCTION("UADDSUBX"); }
-ARM_INST_PTR INTERPRETER_TRANSLATE(uhadd16)(unsigned int inst, int index)   { UNIMPLEMENTED_INSTRUCTION("UHADD16"); }
-ARM_INST_PTR INTERPRETER_TRANSLATE(uhadd8)(unsigned int inst, int index)    { UNIMPLEMENTED_INSTRUCTION("UHADD8"); }
-ARM_INST_PTR INTERPRETER_TRANSLATE(uhaddsubx)(unsigned int inst, int index) { UNIMPLEMENTED_INSTRUCTION("UHADDSUBX"); }
-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(uhadd8)(unsigned int inst, int index)
+{
+    arm_inst* const inst_base = (arm_inst*)AllocBuffer(sizeof(arm_inst) + sizeof(generic_arm_inst));
+    generic_arm_inst* const inst_cream = (generic_arm_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->op1 = BITS(inst, 20, 21);
+    inst_cream->op2 = BITS(inst, 5, 7);
+    inst_cream->Rm  = BITS(inst, 0, 3);
+    inst_cream->Rn  = BITS(inst, 16, 19);
+    inst_cream->Rd  = BITS(inst, 12, 15);
+
+    return inst_base;
+}
+ARM_INST_PTR INTERPRETER_TRANSLATE(uhadd16)(unsigned int inst, int index)
+{
+    return INTERPRETER_TRANSLATE(uhadd8)(inst, index);
+}
+ARM_INST_PTR INTERPRETER_TRANSLATE(uhaddsubx)(unsigned int inst, int index)
+{
+    return INTERPRETER_TRANSLATE(uhadd8)(inst, index);
+}
+ARM_INST_PTR INTERPRETER_TRANSLATE(uhsub8)(unsigned int inst, int index)
+{
+    return INTERPRETER_TRANSLATE(uhadd8)(inst, index);
+}
+ARM_INST_PTR INTERPRETER_TRANSLATE(uhsub16)(unsigned int inst, int index)
+{
+    return INTERPRETER_TRANSLATE(uhadd8)(inst, index);
+}
+ARM_INST_PTR INTERPRETER_TRANSLATE(uhsubaddx)(unsigned int inst, int index)
+{
+    return INTERPRETER_TRANSLATE(uhadd8)(inst, index);
+}
+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));
-        umlal_inst *inst_cream = (umlal_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(umlal_inst));
+    umlal_inst *inst_cream = (umlal_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_base->cond  = BITS(inst, 28, 31);
+    inst_base->idx     = index;
+    inst_base->br     = NON_BRANCH;
+    inst_base->load_r15 = 0;
 
-        inst_cream->S    = BIT(inst, 20);
-        inst_cream->Rm   = BITS(inst, 0, 3);
-        inst_cream->Rs   = BITS(inst, 8, 11);
-        inst_cream->RdHi = BITS(inst, 16, 19);
-        inst_cream->RdLo = BITS(inst, 12, 15);
+    inst_cream->S    = BIT(inst, 20);
+    inst_cream->Rm   = BITS(inst, 0, 3);
+    inst_cream->Rs   = BITS(inst, 8, 11);
+    inst_cream->RdHi = BITS(inst, 16, 19);
+    inst_cream->RdLo = BITS(inst, 12, 15);
 
-        if (CHECK_RM || CHECK_RS) 
-                inst_base->load_r15 = 1;
-        return inst_base;
+    if (CHECK_RM || CHECK_RS) 
+        inst_base->load_r15 = 1;
+
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(umull)(unsigned int inst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(umull_inst));
-        umull_inst *inst_cream = (umull_inst *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(umull_inst));
+    umull_inst *inst_cream = (umull_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_base->cond     = BITS(inst, 28, 31);
+    inst_base->idx      = index;
+    inst_base->br       = NON_BRANCH;
+    inst_base->load_r15 = 0;
 
-        inst_cream->S    = BIT(inst, 20);
-        inst_cream->Rm   = BITS(inst, 0, 3);
-        inst_cream->Rs   = BITS(inst, 8, 11);
-        inst_cream->RdHi = BITS(inst, 16, 19);
-        inst_cream->RdLo = BITS(inst, 12, 15);
+    inst_cream->S    = BIT(inst, 20);
+    inst_cream->Rm   = BITS(inst, 0, 3);
+    inst_cream->Rs   = BITS(inst, 8, 11);
+    inst_cream->RdHi = BITS(inst, 16, 19);
+    inst_cream->RdLo = BITS(inst, 12, 15);
 
-        if (CHECK_RM || CHECK_RS) 
-                inst_base->load_r15 = 1;
-        return inst_base;
+    if (CHECK_RM || CHECK_RS) 
+        inst_base->load_r15 = 1;
+    return inst_base;
 }
 
 ARM_INST_PTR INTERPRETER_TRANSLATE(b_2_thumb)(unsigned int tinst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(b_2_thumb));
-        b_2_thumb *inst_cream = (b_2_thumb *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(b_2_thumb));
+    b_2_thumb *inst_cream = (b_2_thumb *)inst_base->component;
+
+    inst_cream->imm   = ((tinst & 0x3FF) << 1) | ((tinst & (1 << 10)) ? 0xFFFFF800 : 0);
+
+    inst_base->idx    = index;
+    inst_base->br     = DIRECT_BRANCH;
 
-        inst_cream->imm =((tinst & 0x3FF) << 1) | ((tinst & (1 << 10)) ? 0xFFFFF800 : 0);
-        //DEBUG_LOG(ARM11, "In %s, tinst=0x%x, imm=0x%x\n", __FUNCTION__, tinst, inst_cream->imm);
-        inst_base->idx = index;
-        inst_base->br    = DIRECT_BRANCH;
-        return inst_base;
+    return inst_base;
 }
 
 ARM_INST_PTR INTERPRETER_TRANSLATE(b_cond_thumb)(unsigned int tinst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(b_cond_thumb));
-        b_cond_thumb *inst_cream = (b_cond_thumb *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(b_cond_thumb));
+    b_cond_thumb *inst_cream = (b_cond_thumb *)inst_base->component;
 
-        inst_cream->imm = (((tinst & 0x7F) << 1) | ((tinst & (1 << 7)) ?        0xFFFFFF00 : 0));
-        inst_cream->cond = ((tinst >> 8) & 0xf);
-        //DEBUG_LOG(ARM11, "In %s, tinst=0x%x, imm=0x%x, cond=0x%x\n", __FUNCTION__, tinst, inst_cream->imm, inst_cream->cond);
-        inst_base->idx = index;
-        inst_base->br    = DIRECT_BRANCH;
-        return inst_base;
+    inst_cream->imm   = (((tinst & 0x7F) << 1) | ((tinst & (1 << 7)) ?    0xFFFFFF00 : 0));
+    inst_cream->cond  = ((tinst >> 8) & 0xf);
+    inst_base->idx    = index;
+    inst_base->br     = DIRECT_BRANCH;
+
+    return inst_base;
 }
 
 ARM_INST_PTR INTERPRETER_TRANSLATE(bl_1_thumb)(unsigned int tinst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(bl_1_thumb));
-        bl_1_thumb *inst_cream = (bl_1_thumb *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(bl_1_thumb));
+    bl_1_thumb *inst_cream = (bl_1_thumb *)inst_base->component;
 
-        inst_cream->imm = (((tinst & 0x07FF) << 12) | ((tinst & (1 << 10)) ? 0xFF800000 : 0));
-        //DEBUG_LOG(ARM11, "In %s, tinst=0x%x, imm=0x%x\n", __FUNCTION__, tinst, inst_cream->imm);
+    inst_cream->imm    = (((tinst & 0x07FF) << 12) | ((tinst & (1 << 10)) ? 0xFF800000 : 0));
 
-        inst_base->idx   = index;
-        inst_base->br    = NON_BRANCH;
-        return inst_base;
+    inst_base->idx     = index;
+    inst_base->br      = NON_BRANCH;
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(bl_2_thumb)(unsigned int tinst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(bl_2_thumb));
-        bl_2_thumb *inst_cream = (bl_2_thumb *)inst_base->component;
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(bl_2_thumb));
+    bl_2_thumb *inst_cream = (bl_2_thumb *)inst_base->component;
+
+    inst_cream->imm   = (tinst & 0x07FF) << 1;
 
-        inst_cream->imm = (tinst & 0x07FF) << 1;
-        //DEBUG_LOG(ARM11, "In %s, tinst=0x%x, imm=0x%x\n", __FUNCTION__, tinst, inst_cream->imm);
-        inst_base->idx = index;
-        inst_base->br    = DIRECT_BRANCH;
-        return inst_base;
+    inst_base->idx    = index;
+    inst_base->br     = DIRECT_BRANCH;
+    return inst_base;
 }
 ARM_INST_PTR INTERPRETER_TRANSLATE(blx_1_thumb)(unsigned int tinst, int index)
 {
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(blx_1_thumb));
-        blx_1_thumb *inst_cream = (blx_1_thumb *)inst_base->component;
-
-        inst_cream->imm = (tinst & 0x07FF) << 1;
-        //DEBUG_LOG(ARM11, "In %s, tinst=0x%x, imm=0x%x\n", __FUNCTION__, tinst, inst_cream->imm);
-        inst_cream->instr = tinst;
-        inst_base->idx   = index;
-        inst_base->br    = DIRECT_BRANCH;
-        return inst_base;
-}
-
-ARM_INST_PTR INTERPRETER_TRANSLATE(uqadd16)(unsigned int inst, int index)   { UNIMPLEMENTED_INSTRUCTION("UQADD16"); }
-ARM_INST_PTR INTERPRETER_TRANSLATE(uqadd8)(unsigned int inst, int index)    { UNIMPLEMENTED_INSTRUCTION("UQADD8"); }
-ARM_INST_PTR INTERPRETER_TRANSLATE(uqaddsubx)(unsigned int inst, int index) { UNIMPLEMENTED_INSTRUCTION("UQADDSUBX"); }
-ARM_INST_PTR INTERPRETER_TRANSLATE(uqsub16)(unsigned int inst, int index)   { UNIMPLEMENTED_INSTRUCTION("UQSUB16"); }
-ARM_INST_PTR INTERPRETER_TRANSLATE(uqsub8)(unsigned int inst, int index)    { UNIMPLEMENTED_INSTRUCTION("UQSUB8"); }
-ARM_INST_PTR INTERPRETER_TRANSLATE(uqsubaddx)(unsigned int inst, int index) { UNIMPLEMENTED_INSTRUCTION("UQSUBADDX"); }
-ARM_INST_PTR INTERPRETER_TRANSLATE(usad8)(unsigned int inst, int index)     { UNIMPLEMENTED_INSTRUCTION("USAD8"); }
-ARM_INST_PTR INTERPRETER_TRANSLATE(usada8)(unsigned int inst, int index)    { UNIMPLEMENTED_INSTRUCTION("USADA8"); }
+    arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(blx_1_thumb));
+    blx_1_thumb *inst_cream = (blx_1_thumb *)inst_base->component;
+
+    inst_cream->imm   = (tinst & 0x07FF) << 1;
+    inst_cream->instr = tinst;
+
+    inst_base->idx    = index;
+    inst_base->br     = DIRECT_BRANCH;
+    return inst_base;
+}
+
+ARM_INST_PTR INTERPRETER_TRANSLATE(uqadd8)(unsigned int inst, int index)
+{
+    arm_inst* const inst_base = (arm_inst*)AllocBuffer(sizeof(arm_inst) + sizeof(generic_arm_inst));
+    generic_arm_inst* const inst_cream = (generic_arm_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, 0, 3);
+    inst_cream->Rn  = BITS(inst, 16, 19);
+    inst_cream->Rd  = BITS(inst, 12, 15);
+    inst_cream->op1 = BITS(inst, 20, 21);
+    inst_cream->op2 = BITS(inst, 5, 7);
+
+    return inst_base;
+}
+ARM_INST_PTR INTERPRETER_TRANSLATE(uqadd16)(unsigned int inst, int index)
+{
+    return INTERPRETER_TRANSLATE(uqadd8)(inst, index);
+}
+ARM_INST_PTR INTERPRETER_TRANSLATE(uqaddsubx)(unsigned int inst, int index)
+{
+    return INTERPRETER_TRANSLATE(uqadd8)(inst, index);
+}
+ARM_INST_PTR INTERPRETER_TRANSLATE(uqsub8)(unsigned int inst, int index)
+{
+    return INTERPRETER_TRANSLATE(uqadd8)(inst, index);
+}
+ARM_INST_PTR INTERPRETER_TRANSLATE(uqsub16)(unsigned int inst, int index)
+{
+    return INTERPRETER_TRANSLATE(uqadd8)(inst, index);
+}
+ARM_INST_PTR INTERPRETER_TRANSLATE(uqsubaddx)(unsigned int inst, int index)
+{
+    return INTERPRETER_TRANSLATE(uqadd8)(inst, index);
+}
+ARM_INST_PTR INTERPRETER_TRANSLATE(usada8)(unsigned int inst, int index)
+{
+    arm_inst* const inst_base = (arm_inst*)AllocBuffer(sizeof(arm_inst) + sizeof(generic_arm_inst));
+    generic_arm_inst* const inst_cream = (generic_arm_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->op1 = BITS(inst, 20, 24);
+    inst_cream->op2 = BITS(inst, 5, 7);
+    inst_cream->Rm  = BITS(inst, 8, 11);
+    inst_cream->Rn  = BITS(inst, 0, 3);
+    inst_cream->Ra  = BITS(inst, 12, 15);
+
+    return inst_base;
+}
+ARM_INST_PTR INTERPRETER_TRANSLATE(usad8)(unsigned int inst, int index)
+{
+    return INTERPRETER_TRANSLATE(usada8)(inst, index);
+}
 ARM_INST_PTR INTERPRETER_TRANSLATE(usat)(unsigned int inst, int index)      { UNIMPLEMENTED_INSTRUCTION("USAT"); }
 ARM_INST_PTR INTERPRETER_TRANSLATE(usat16)(unsigned int inst, int index)    { UNIMPLEMENTED_INSTRUCTION("USAT16"); }
 ARM_INST_PTR INTERPRETER_TRANSLATE(usub16)(unsigned int inst, int index)    { UNIMPLEMENTED_INSTRUCTION("USUB16"); }
 ARM_INST_PTR INTERPRETER_TRANSLATE(usub8)(unsigned int inst, int index)     { UNIMPLEMENTED_INSTRUCTION("USUB8"); }
 ARM_INST_PTR INTERPRETER_TRANSLATE(usubaddx)(unsigned int inst, int index)  { UNIMPLEMENTED_INSTRUCTION("USUBADDX"); }
-ARM_INST_PTR INTERPRETER_TRANSLATE(uxtab16)(unsigned int inst, int index)   { UNIMPLEMENTED_INSTRUCTION("UXTAB16"); }
-ARM_INST_PTR INTERPRETER_TRANSLATE(uxtb16)(unsigned int inst, int index)    { UNIMPLEMENTED_INSTRUCTION("UXTB16"); }
 
+ARM_INST_PTR INTERPRETER_TRANSLATE(uxtab16)(unsigned int inst, int index)
+{
+    arm_inst* const inst_base = (arm_inst*)AllocBuffer(sizeof(arm_inst) + sizeof(uxtab_inst));
+    uxtab_inst* const inst_cream = (uxtab_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;
 
-/* Floating point VFPv3 structures and instructions */
+    inst_cream->Rm     = BITS(inst, 0, 3);
+    inst_cream->Rn     = BITS(inst, 16, 19);
+    inst_cream->Rd     = BITS(inst, 12, 15);
+    inst_cream->rotate = BITS(inst, 10, 11);
+
+    return inst_base;
+}
+ARM_INST_PTR INTERPRETER_TRANSLATE(uxtb16)(unsigned int inst, int index)
+{
+    return INTERPRETER_TRANSLATE(uxtab16)(inst, index);
+}
+
+// Floating point VFPv3 structures and instructions
 
 #define VFP_INTERPRETER_STRUCT
 #include "core/arm/skyeye_common/vfp/vfpinstr.cpp"
@@ -3141,321 +3166,288 @@ ARM_INST_PTR INTERPRETER_TRANSLATE(uxtb16)(unsigned int inst, int index)    { UN
 #include "core/arm/skyeye_common/vfp/vfpinstr.cpp"
 #undef VFP_INTERPRETER_TRANS
 
-
-
 typedef ARM_INST_PTR (*transop_fp_t)(unsigned int, int);
 
 const transop_fp_t arm_instruction_trans[] = {
-        #define VFP_INTERPRETER_TABLE
-        #include "core/arm/skyeye_common/vfp/vfpinstr.cpp"
-        #undef VFP_INTERPRETER_TABLE
-        INTERPRETER_TRANSLATE(srs),
-        INTERPRETER_TRANSLATE(rfe),
-        INTERPRETER_TRANSLATE(bkpt),
-        INTERPRETER_TRANSLATE(blx),
-        INTERPRETER_TRANSLATE(cps),
-        INTERPRETER_TRANSLATE(pld),
-        INTERPRETER_TRANSLATE(setend),
-        INTERPRETER_TRANSLATE(clrex),
-        INTERPRETER_TRANSLATE(rev16),
-        INTERPRETER_TRANSLATE(usad8),
-        INTERPRETER_TRANSLATE(sxtb),
-        INTERPRETER_TRANSLATE(uxtb),
-        INTERPRETER_TRANSLATE(sxth),
-        INTERPRETER_TRANSLATE(sxtb16),
-        INTERPRETER_TRANSLATE(uxth),
-        INTERPRETER_TRANSLATE(uxtb16),
-        INTERPRETER_TRANSLATE(cpy),
-        INTERPRETER_TRANSLATE(uxtab),
-        INTERPRETER_TRANSLATE(ssub8),
-        INTERPRETER_TRANSLATE(shsub8),
-        INTERPRETER_TRANSLATE(ssubaddx),
-        INTERPRETER_TRANSLATE(strex),
-        INTERPRETER_TRANSLATE(strexb),
-        INTERPRETER_TRANSLATE(swp),
-        INTERPRETER_TRANSLATE(swpb),
-        INTERPRETER_TRANSLATE(ssub16),
-        INTERPRETER_TRANSLATE(ssat16),
-        INTERPRETER_TRANSLATE(shsubaddx),
-        INTERPRETER_TRANSLATE(qsubaddx),
-        INTERPRETER_TRANSLATE(shaddsubx),
-        INTERPRETER_TRANSLATE(shadd8),
-        INTERPRETER_TRANSLATE(shadd16),
-        INTERPRETER_TRANSLATE(sel),
-        INTERPRETER_TRANSLATE(saddsubx),
-        INTERPRETER_TRANSLATE(sadd8),
-        INTERPRETER_TRANSLATE(sadd16),
-        INTERPRETER_TRANSLATE(shsub16),
-        INTERPRETER_TRANSLATE(umaal),
-        INTERPRETER_TRANSLATE(uxtab16),
-        INTERPRETER_TRANSLATE(usubaddx),
-        INTERPRETER_TRANSLATE(usub8),
-        INTERPRETER_TRANSLATE(usub16),
-        INTERPRETER_TRANSLATE(usat16),
-        INTERPRETER_TRANSLATE(usada8),
-        INTERPRETER_TRANSLATE(uqsubaddx),
-        INTERPRETER_TRANSLATE(uqsub8),
-        INTERPRETER_TRANSLATE(uqsub16),
-        INTERPRETER_TRANSLATE(uqaddsubx),
-        INTERPRETER_TRANSLATE(uqadd8),
-        INTERPRETER_TRANSLATE(uqadd16),
-        INTERPRETER_TRANSLATE(sxtab),
-        INTERPRETER_TRANSLATE(uhsubaddx),
-        INTERPRETER_TRANSLATE(uhsub8),
-        INTERPRETER_TRANSLATE(uhsub16),
-        INTERPRETER_TRANSLATE(uhaddsubx),
-        INTERPRETER_TRANSLATE(uhadd8),
-        INTERPRETER_TRANSLATE(uhadd16),
-        INTERPRETER_TRANSLATE(uaddsubx),
-        INTERPRETER_TRANSLATE(uadd8),
-        INTERPRETER_TRANSLATE(uadd16),
-        INTERPRETER_TRANSLATE(sxtah),
-        INTERPRETER_TRANSLATE(sxtab16),
-        INTERPRETER_TRANSLATE(qadd8),
-        INTERPRETER_TRANSLATE(bxj),
-        INTERPRETER_TRANSLATE(clz),
-        INTERPRETER_TRANSLATE(uxtah),
-        INTERPRETER_TRANSLATE(bx),
-        INTERPRETER_TRANSLATE(rev),
-        INTERPRETER_TRANSLATE(blx),
-        INTERPRETER_TRANSLATE(revsh),
-        INTERPRETER_TRANSLATE(qadd),
-        INTERPRETER_TRANSLATE(qadd16),
-        INTERPRETER_TRANSLATE(qaddsubx),
-        INTERPRETER_TRANSLATE(ldrex),
-        INTERPRETER_TRANSLATE(qdadd),
-        INTERPRETER_TRANSLATE(qdsub),
-        INTERPRETER_TRANSLATE(qsub),
-        INTERPRETER_TRANSLATE(ldrexb),
-        INTERPRETER_TRANSLATE(qsub8),
-        INTERPRETER_TRANSLATE(qsub16),
-        INTERPRETER_TRANSLATE(smuad),
-        INTERPRETER_TRANSLATE(smmul),
-        INTERPRETER_TRANSLATE(smusd),
-        INTERPRETER_TRANSLATE(smlsd),
-        INTERPRETER_TRANSLATE(smlsld),
-        INTERPRETER_TRANSLATE(smmla),
-        INTERPRETER_TRANSLATE(smmls),
-        INTERPRETER_TRANSLATE(smlald),
-        INTERPRETER_TRANSLATE(smlad),
-        INTERPRETER_TRANSLATE(smlaw),
-        INTERPRETER_TRANSLATE(smulw),
-        INTERPRETER_TRANSLATE(pkhtb),
-        INTERPRETER_TRANSLATE(pkhbt),
-        INTERPRETER_TRANSLATE(smul),
-        INTERPRETER_TRANSLATE(smlalxy),
-        INTERPRETER_TRANSLATE(smla),
-        INTERPRETER_TRANSLATE(mcrr),
-        INTERPRETER_TRANSLATE(mrrc),
-        INTERPRETER_TRANSLATE(cmp),
-        INTERPRETER_TRANSLATE(tst),
-        INTERPRETER_TRANSLATE(teq),
-        INTERPRETER_TRANSLATE(cmn),
-        INTERPRETER_TRANSLATE(smull),
-        INTERPRETER_TRANSLATE(umull),
-        INTERPRETER_TRANSLATE(umlal),
-        INTERPRETER_TRANSLATE(smlal),
-        INTERPRETER_TRANSLATE(mul),
-        INTERPRETER_TRANSLATE(mla),
-        INTERPRETER_TRANSLATE(ssat),
-        INTERPRETER_TRANSLATE(usat),
-        INTERPRETER_TRANSLATE(mrs),
-        INTERPRETER_TRANSLATE(msr),
-        INTERPRETER_TRANSLATE(and),
-        INTERPRETER_TRANSLATE(bic),
-        INTERPRETER_TRANSLATE(ldm),
-        INTERPRETER_TRANSLATE(eor),
-        INTERPRETER_TRANSLATE(add),
-        INTERPRETER_TRANSLATE(rsb),
-        INTERPRETER_TRANSLATE(rsc),
-        INTERPRETER_TRANSLATE(sbc),
-        INTERPRETER_TRANSLATE(adc),
-        INTERPRETER_TRANSLATE(sub),
-        INTERPRETER_TRANSLATE(orr),
-        INTERPRETER_TRANSLATE(mvn),
-        INTERPRETER_TRANSLATE(mov),
-        INTERPRETER_TRANSLATE(stm),
-        INTERPRETER_TRANSLATE(ldm),
-        INTERPRETER_TRANSLATE(ldrsh),
-        INTERPRETER_TRANSLATE(stm),
-        INTERPRETER_TRANSLATE(ldm),
-        INTERPRETER_TRANSLATE(ldrsb),
-        INTERPRETER_TRANSLATE(strd),
-        INTERPRETER_TRANSLATE(ldrh),
-        INTERPRETER_TRANSLATE(strh),
-        INTERPRETER_TRANSLATE(ldrd),
-        INTERPRETER_TRANSLATE(strt),
-        INTERPRETER_TRANSLATE(strbt),
-        INTERPRETER_TRANSLATE(ldrbt),
-        INTERPRETER_TRANSLATE(ldrt),
-        INTERPRETER_TRANSLATE(mrc),
-        INTERPRETER_TRANSLATE(mcr),
-        INTERPRETER_TRANSLATE(msr),
-        INTERPRETER_TRANSLATE(ldrb),
-        INTERPRETER_TRANSLATE(strb),
-        INTERPRETER_TRANSLATE(ldr),
-        INTERPRETER_TRANSLATE(ldrcond),
-        INTERPRETER_TRANSLATE(str),
-        INTERPRETER_TRANSLATE(cdp),
-        INTERPRETER_TRANSLATE(stc),
-        INTERPRETER_TRANSLATE(ldc),
-        INTERPRETER_TRANSLATE(swi),
-        INTERPRETER_TRANSLATE(bbl),
-        /* All the thumb instructions should be placed the end of table */
-        INTERPRETER_TRANSLATE(b_2_thumb), 
-        INTERPRETER_TRANSLATE(b_cond_thumb), 
-        INTERPRETER_TRANSLATE(bl_1_thumb), 
-        INTERPRETER_TRANSLATE(bl_2_thumb),
-        INTERPRETER_TRANSLATE(blx_1_thumb)
+    INTERPRETER_TRANSLATE(vmla),
+    INTERPRETER_TRANSLATE(vmls),
+    INTERPRETER_TRANSLATE(vnmla),
+    INTERPRETER_TRANSLATE(vnmla),
+    INTERPRETER_TRANSLATE(vnmls),
+    INTERPRETER_TRANSLATE(vnmul),
+    INTERPRETER_TRANSLATE(vmul),
+    INTERPRETER_TRANSLATE(vadd),
+    INTERPRETER_TRANSLATE(vsub),
+    INTERPRETER_TRANSLATE(vdiv),
+    INTERPRETER_TRANSLATE(vmovi),
+    INTERPRETER_TRANSLATE(vmovr),
+    INTERPRETER_TRANSLATE(vabs),
+    INTERPRETER_TRANSLATE(vneg),
+    INTERPRETER_TRANSLATE(vsqrt),
+    INTERPRETER_TRANSLATE(vcmp),
+    INTERPRETER_TRANSLATE(vcmp2),
+    INTERPRETER_TRANSLATE(vcvtbds),
+    INTERPRETER_TRANSLATE(vcvtbff),
+    INTERPRETER_TRANSLATE(vcvtbfi),
+    INTERPRETER_TRANSLATE(vmovbrs),
+    INTERPRETER_TRANSLATE(vmsr),
+    INTERPRETER_TRANSLATE(vmovbrc),
+    INTERPRETER_TRANSLATE(vmrs),
+    INTERPRETER_TRANSLATE(vmovbcr),
+    INTERPRETER_TRANSLATE(vmovbrrss),
+    INTERPRETER_TRANSLATE(vmovbrrd),
+    INTERPRETER_TRANSLATE(vstr),
+    INTERPRETER_TRANSLATE(vpush),
+    INTERPRETER_TRANSLATE(vstm),
+    INTERPRETER_TRANSLATE(vpop),
+    INTERPRETER_TRANSLATE(vldr),
+    INTERPRETER_TRANSLATE(vldm),
+
+    INTERPRETER_TRANSLATE(srs),
+    INTERPRETER_TRANSLATE(rfe),
+    INTERPRETER_TRANSLATE(bkpt),
+    INTERPRETER_TRANSLATE(blx),
+    INTERPRETER_TRANSLATE(cps),
+    INTERPRETER_TRANSLATE(pld),
+    INTERPRETER_TRANSLATE(setend),
+    INTERPRETER_TRANSLATE(clrex),
+    INTERPRETER_TRANSLATE(rev16),
+    INTERPRETER_TRANSLATE(usad8),
+    INTERPRETER_TRANSLATE(sxtb),
+    INTERPRETER_TRANSLATE(uxtb),
+    INTERPRETER_TRANSLATE(sxth),
+    INTERPRETER_TRANSLATE(sxtb16),
+    INTERPRETER_TRANSLATE(uxth),
+    INTERPRETER_TRANSLATE(uxtb16),
+    INTERPRETER_TRANSLATE(cpy),
+    INTERPRETER_TRANSLATE(uxtab),
+    INTERPRETER_TRANSLATE(ssub8),
+    INTERPRETER_TRANSLATE(shsub8),
+    INTERPRETER_TRANSLATE(ssubaddx),
+    INTERPRETER_TRANSLATE(strex),
+    INTERPRETER_TRANSLATE(strexb),
+    INTERPRETER_TRANSLATE(swp),
+    INTERPRETER_TRANSLATE(swpb),
+    INTERPRETER_TRANSLATE(ssub16),
+    INTERPRETER_TRANSLATE(ssat16),
+    INTERPRETER_TRANSLATE(shsubaddx),
+    INTERPRETER_TRANSLATE(qsubaddx),
+    INTERPRETER_TRANSLATE(shaddsubx),
+    INTERPRETER_TRANSLATE(shadd8),
+    INTERPRETER_TRANSLATE(shadd16),
+    INTERPRETER_TRANSLATE(sel),
+    INTERPRETER_TRANSLATE(saddsubx),
+    INTERPRETER_TRANSLATE(sadd8),
+    INTERPRETER_TRANSLATE(sadd16),
+    INTERPRETER_TRANSLATE(shsub16),
+    INTERPRETER_TRANSLATE(umaal),
+    INTERPRETER_TRANSLATE(uxtab16),
+    INTERPRETER_TRANSLATE(usubaddx),
+    INTERPRETER_TRANSLATE(usub8),
+    INTERPRETER_TRANSLATE(usub16),
+    INTERPRETER_TRANSLATE(usat16),
+    INTERPRETER_TRANSLATE(usada8),
+    INTERPRETER_TRANSLATE(uqsubaddx),
+    INTERPRETER_TRANSLATE(uqsub8),
+    INTERPRETER_TRANSLATE(uqsub16),
+    INTERPRETER_TRANSLATE(uqaddsubx),
+    INTERPRETER_TRANSLATE(uqadd8),
+    INTERPRETER_TRANSLATE(uqadd16),
+    INTERPRETER_TRANSLATE(sxtab),
+    INTERPRETER_TRANSLATE(uhsubaddx),
+    INTERPRETER_TRANSLATE(uhsub8),
+    INTERPRETER_TRANSLATE(uhsub16),
+    INTERPRETER_TRANSLATE(uhaddsubx),
+    INTERPRETER_TRANSLATE(uhadd8),
+    INTERPRETER_TRANSLATE(uhadd16),
+    INTERPRETER_TRANSLATE(uaddsubx),
+    INTERPRETER_TRANSLATE(uadd8),
+    INTERPRETER_TRANSLATE(uadd16),
+    INTERPRETER_TRANSLATE(sxtah),
+    INTERPRETER_TRANSLATE(sxtab16),
+    INTERPRETER_TRANSLATE(qadd8),
+    INTERPRETER_TRANSLATE(bxj),
+    INTERPRETER_TRANSLATE(clz),
+    INTERPRETER_TRANSLATE(uxtah),
+    INTERPRETER_TRANSLATE(bx),
+    INTERPRETER_TRANSLATE(rev),
+    INTERPRETER_TRANSLATE(blx),
+    INTERPRETER_TRANSLATE(revsh),
+    INTERPRETER_TRANSLATE(qadd),
+    INTERPRETER_TRANSLATE(qadd16),
+    INTERPRETER_TRANSLATE(qaddsubx),
+    INTERPRETER_TRANSLATE(ldrex),
+    INTERPRETER_TRANSLATE(qdadd),
+    INTERPRETER_TRANSLATE(qdsub),
+    INTERPRETER_TRANSLATE(qsub),
+    INTERPRETER_TRANSLATE(ldrexb),
+    INTERPRETER_TRANSLATE(qsub8),
+    INTERPRETER_TRANSLATE(qsub16),
+    INTERPRETER_TRANSLATE(smuad),
+    INTERPRETER_TRANSLATE(smmul),
+    INTERPRETER_TRANSLATE(smusd),
+    INTERPRETER_TRANSLATE(smlsd),
+    INTERPRETER_TRANSLATE(smlsld),
+    INTERPRETER_TRANSLATE(smmla),
+    INTERPRETER_TRANSLATE(smmls),
+    INTERPRETER_TRANSLATE(smlald),
+    INTERPRETER_TRANSLATE(smlad),
+    INTERPRETER_TRANSLATE(smlaw),
+    INTERPRETER_TRANSLATE(smulw),
+    INTERPRETER_TRANSLATE(pkhtb),
+    INTERPRETER_TRANSLATE(pkhbt),
+    INTERPRETER_TRANSLATE(smul),
+    INTERPRETER_TRANSLATE(smlalxy),
+    INTERPRETER_TRANSLATE(smla),
+    INTERPRETER_TRANSLATE(mcrr),
+    INTERPRETER_TRANSLATE(mrrc),
+    INTERPRETER_TRANSLATE(cmp),
+    INTERPRETER_TRANSLATE(tst),
+    INTERPRETER_TRANSLATE(teq),
+    INTERPRETER_TRANSLATE(cmn),
+    INTERPRETER_TRANSLATE(smull),
+    INTERPRETER_TRANSLATE(umull),
+    INTERPRETER_TRANSLATE(umlal),
+    INTERPRETER_TRANSLATE(smlal),
+    INTERPRETER_TRANSLATE(mul),
+    INTERPRETER_TRANSLATE(mla),
+    INTERPRETER_TRANSLATE(ssat),
+    INTERPRETER_TRANSLATE(usat),
+    INTERPRETER_TRANSLATE(mrs),
+    INTERPRETER_TRANSLATE(msr),
+    INTERPRETER_TRANSLATE(and),
+    INTERPRETER_TRANSLATE(bic),
+    INTERPRETER_TRANSLATE(ldm),
+    INTERPRETER_TRANSLATE(eor),
+    INTERPRETER_TRANSLATE(add),
+    INTERPRETER_TRANSLATE(rsb),
+    INTERPRETER_TRANSLATE(rsc),
+    INTERPRETER_TRANSLATE(sbc),
+    INTERPRETER_TRANSLATE(adc),
+    INTERPRETER_TRANSLATE(sub),
+    INTERPRETER_TRANSLATE(orr),
+    INTERPRETER_TRANSLATE(mvn),
+    INTERPRETER_TRANSLATE(mov),
+    INTERPRETER_TRANSLATE(stm),
+    INTERPRETER_TRANSLATE(ldm),
+    INTERPRETER_TRANSLATE(ldrsh),
+    INTERPRETER_TRANSLATE(stm),
+    INTERPRETER_TRANSLATE(ldm),
+    INTERPRETER_TRANSLATE(ldrsb),
+    INTERPRETER_TRANSLATE(strd),
+    INTERPRETER_TRANSLATE(ldrh),
+    INTERPRETER_TRANSLATE(strh),
+    INTERPRETER_TRANSLATE(ldrd),
+    INTERPRETER_TRANSLATE(strt),
+    INTERPRETER_TRANSLATE(strbt),
+    INTERPRETER_TRANSLATE(ldrbt),
+    INTERPRETER_TRANSLATE(ldrt),
+    INTERPRETER_TRANSLATE(mrc),
+    INTERPRETER_TRANSLATE(mcr),
+    INTERPRETER_TRANSLATE(msr),
+    INTERPRETER_TRANSLATE(ldrb),
+    INTERPRETER_TRANSLATE(strb),
+    INTERPRETER_TRANSLATE(ldr),
+    INTERPRETER_TRANSLATE(ldrcond),
+    INTERPRETER_TRANSLATE(str),
+    INTERPRETER_TRANSLATE(cdp),
+    INTERPRETER_TRANSLATE(stc),
+    INTERPRETER_TRANSLATE(ldc),
+    INTERPRETER_TRANSLATE(swi),
+    INTERPRETER_TRANSLATE(bbl),
+    // All the thumb instructions should be placed the end of table
+    INTERPRETER_TRANSLATE(b_2_thumb), 
+    INTERPRETER_TRANSLATE(b_cond_thumb), 
+    INTERPRETER_TRANSLATE(bl_1_thumb), 
+    INTERPRETER_TRANSLATE(bl_2_thumb),
+    INTERPRETER_TRANSLATE(blx_1_thumb)
 };
 
 typedef std::unordered_map<u32, int> bb_map;
 bb_map CreamCache;
 
-//#define USE_DUMMY_CACHE
-
-#ifdef USE_DUMMY_CACHE
-unsigned int DummyCache[0x100000];
-#endif
-
-void insert_bb(unsigned int addr, int start)
-{
-#ifdef USE_DUMMY_CACHE
-        DummyCache[addr] = start;
-#else
-        CreamCache[addr] = start;
-#endif
+void insert_bb(unsigned int addr, int start) {
+    CreamCache[addr] = start;
 }
 
 #define TRANS_THRESHOLD                 65000
-int find_bb(unsigned int addr, int &start)
-{
-        int ret = -1;
-#ifdef USE_DUMMY_CACHE
-        start = DummyCache[addr];
-        if (start) {
-                ret = 0;
-        } else
-                ret = -1;
-#else
-        bb_map::const_iterator it = CreamCache.find(addr);
-        if (it != CreamCache.end()) {
-                start = static_cast<int>(it->second);
-                ret = 0;
-#if HYBRID_MODE
-#if PROFILE
-#else
-                /* increase the bb counter */
-                if(get_bb_prof(cpu, addr, 1) == TRANS_THRESHOLD){
-                        push_to_compiled(cpu, addr);
-                }
-#endif
-#endif
-        } else {
-                ret = -1;
-        }
-#endif
-        return ret;
+int find_bb(unsigned int addr, int &start) {
+    int ret = -1;
+    bb_map::const_iterator it = CreamCache.find(addr);
+    if (it != CreamCache.end()) {
+        start = static_cast<int>(it->second);
+        ret = 0;
+    } else {
+        ret = -1;
+    }
+    return ret;
 }
 
-
 enum {
-        FETCH_SUCCESS,
-        FETCH_FAILURE
+    FETCH_SUCCESS,
+    FETCH_FAILURE
 };
+
 static tdstate decode_thumb_instr(arm_processor *cpu, uint32_t inst, addr_t addr, uint32_t *arm_inst, uint32_t* inst_size, ARM_INST_PTR* ptr_inst_base){
-        /* Check if in Thumb mode.  */
-        tdstate ret;
-        ret = thumb_translate (addr, inst, arm_inst, inst_size);
-        if(ret == t_branch){
-                /* FIXME, endian should be judged */
-                uint32 tinstr;
-                if((addr & 0x3) != 0)
-                        tinstr = inst >> 16;
-                else
-                        tinstr = inst & 0xFFFF;
-
-                //tinstr = inst & 0xFFFF;
-                int inst_index;
-                /* table_length */
-                int table_length = sizeof(arm_instruction_trans) / sizeof(transop_fp_t);
-
-                switch((tinstr & 0xF800) >> 11){
-                /* we will translate the thumb instruction directly here */
-                /* we will translate the thumb instruction directly here */
-                case 26:
-                case 27:
-                        if (((tinstr & 0x0F00) != 0x0E00) && ((tinstr & 0x0F00) != 0x0F00)){
-                                uint32 cond = (tinstr & 0x0F00) >> 8;
-                                inst_index = table_length - 4;
-                                //DEBUG_LOG(ARM11, "In %s, tinstr=0x%x, blx 1 thumb index=%d\n", __FUNCTION__, tinstr, inst_index);
-                                *ptr_inst_base = arm_instruction_trans[inst_index](tinstr, inst_index);
-                        }
-                        else{
-                        /* something wrong */
-                                LOG_ERROR(Core_ARM11, "thumb decoder error");
-                        }
-                        break;
-                case 28:
-                        /* Branch 2, unconditional branch */
-                        inst_index = table_length - 5;
-                        //DEBUG_LOG(ARM11, "In %s, tinstr=0x%x, blx 1 thumb index=%d\n", __FUNCTION__, tinstr, inst_index);
-                        *ptr_inst_base = arm_instruction_trans[inst_index](tinstr, inst_index);
-                        break;
-
-                case 8:
-                case 29:
-                        /* For BLX 1 thumb instruction*/
-                        inst_index = table_length - 1;
-                        //DEBUG_LOG(ARM11, "In %s, tinstr=0x%x, blx 1 thumb index=%d, pc=0x%x\n", __FUNCTION__, tinstr, inst_index, cpu->translate_pc);
-                        *ptr_inst_base = arm_instruction_trans[inst_index](tinstr, inst_index);
-                        break;
-                case 30:
-                        /* For BL 1 thumb instruction*/
-                        inst_index = table_length - 3;
-                        //DEBUG_LOG(ARM11, "In %s, tinstr=0x%x, bl 1 thumb index=%d, pc=0x%x\n", __FUNCTION__, tinstr, inst_index, cpu->translate_pc);
-                        *ptr_inst_base = arm_instruction_trans[inst_index](tinstr, inst_index);
-                        break;
-                case 31:
-                        /* For BL 2 thumb instruction*/
-                        inst_index = table_length - 2;
-                        //DEBUG_LOG(ARM11, "In %s, tinstr=0x%x, bl 2 thumb index=%d, px=0x%x\n", __FUNCTION__, tinstr, inst_index, cpu->translate_pc);
-                        *ptr_inst_base = arm_instruction_trans[inst_index](tinstr, inst_index);
-                        break;
-                default:
-                        ret = t_undefined;
-                        break;
-                }
-        }
-        return ret;
-}
-
-#if 0
-int FetchInst(cpu_t *core, unsigned int &inst)
-{
-        //arm_processor *cpu = (arm_processor *)get_cast_conf_obj(core->cpu_data, "arm_core_t");
-        arm_processor *cpu = (arm_processor *)(core->cpu_data->obj);
-//      fault_t fault = interpreter_read_memory(cpu->translate_pc, inst, 32);
-        fault_t fault = interpreter_fetch(core, cpu->translate_pc, inst, 32);
-        if (!core->is_user_mode) {
-                if (fault) {
-                        cpu->abortSig = true;
-                        cpu->Aborted = ARMul_PrefetchAbortV;
-                        cpu->AbortAddr = cpu->translate_pc;
-                        cpu->CP15[CP15(CP15_INSTR_FAULT_STATUS)] = fault & 0xff;
-                        cpu->CP15[CP15(CP15_FAULT_ADDRESS)] = cpu->translate_pc;
-                        return FETCH_FAILURE;
-                }
-        }
-        return FETCH_SUCCESS;
+    // Check if in Thumb mode
+    tdstate ret = thumb_translate (addr, inst, arm_inst, inst_size);
+    if(ret == t_branch){
+        // TODO: FIXME, endian should be judged
+        uint32 tinstr;
+        if((addr & 0x3) != 0)
+            tinstr = inst >> 16;
+        else
+            tinstr = inst & 0xFFFF;
+
+        int inst_index;
+        int table_length = sizeof(arm_instruction_trans) / sizeof(transop_fp_t);
+
+        switch((tinstr & 0xF800) >> 11){
+        case 26:
+        case 27:
+            if (((tinstr & 0x0F00) != 0x0E00) && ((tinstr & 0x0F00) != 0x0F00)){
+                uint32 cond = (tinstr & 0x0F00) >> 8;
+                inst_index = table_length - 4;
+                *ptr_inst_base = arm_instruction_trans[inst_index](tinstr, inst_index);
+            } else {
+                LOG_ERROR(Core_ARM11, "thumb decoder error");
+            }
+            break;
+        case 28:
+            // Branch 2, unconditional branch
+            inst_index = table_length - 5;
+            *ptr_inst_base = arm_instruction_trans[inst_index](tinstr, inst_index);
+            break;
+
+        case 8:
+        case 29:
+            // For BLX 1 thumb instruction
+            inst_index = table_length - 1;
+            *ptr_inst_base = arm_instruction_trans[inst_index](tinstr, inst_index);
+            break;
+        case 30:
+            // For BL 1 thumb instruction
+            inst_index = table_length - 3;
+            *ptr_inst_base = arm_instruction_trans[inst_index](tinstr, inst_index);
+            break;
+        case 31:
+            // For BL 2 thumb instruction
+            inst_index = table_length - 2;
+            *ptr_inst_base = arm_instruction_trans[inst_index](tinstr, inst_index);
+            break;
+        default:
+            ret = t_undefined;
+            break;
+        }
+    }
+    return ret;
 }
-#endif
 
 unsigned int *InstLength;
 
 enum {
-        KEEP_GOING,
-        FETCH_EXCEPTION
+    KEEP_GOING,
+    FETCH_EXCEPTION
 };
 
 typedef struct instruction_set_encoding_item ISEITEM;
@@ -3464,274 +3456,148 @@ extern const ISEITEM arm_instruction[];
 
 vector<uint64_t> code_page_set;
 
-void flush_bb(uint32_t addr)
-{
-        bb_map::iterator it;
-        uint32_t start;
-
-        addr  &= 0xfffff000;
-        for (it = CreamCache.begin(); it != CreamCache.end(); ) {
-                start = static_cast<uint32_t>(it->first);
-                //start = (start >> 12) << 12;
-                start &= 0xfffff000;
-                if (start == addr) {
-                        //DEBUG_LOG(ARM11, "[ERASE][0x%08x]\n", static_cast<int>(it->first));
-                        CreamCache.erase(it++);
-                } else
-                        ++it;
-        }
-
-        //DEBUG_LOG(ARM11, "flush bb @ %x\n", addr);
-}
-
-//static uint32_t get_bank_addr(void *addr)
-//{
-//      uint64_t address = (uint64_t)addr;
-//      uint64_t bank0 = get_dma_addr(BANK0_START);
-//      if ((address >= bank0) && (address < (bank0 + BANK0_SIZE))) {
-//              //DEBUG_LOG(ARM11, "1.addr is %llx\n", addr);
-//              return ((uint64_t)addr - bank0) + BANK0_START;
-//      }
-//      return 0;
-//}
-
-/* shenoubang add win32 2012-6-12 */
-//#ifndef __WIN32__
-//static void flush_code_cache(int signal_number, siginfo_t *si, void *unused)
-//{
-//      DEBUG_LOG(ARM11, "in %s, addr=0x%llx\n", __FUNCTION__, si->si_addr);
-//      uint64_t addr = (uint64_t)si->si_addr;
-//      addr = (addr >> 12) << 12;
-//      skyeye_backtrace();
-//      #if 0
-//      if (addr == 0) {
-//              return;
-//      }
-//      const vector<uint64_t>::iterator it = find(code_page_set.begin(), 
-//                                                 code_page_set.end(),
-//                                                 (uint64_t)addr);
-//      if (it != code_page_set.end()) {
-//              code_page_set.erase(it);
-//      }
-//      mprotect((void *)addr, 4096, PROT_READ | PROT_WRITE);
-//      //DEBUG_LOG(ARM11, "[flush][ADDR:0x%08llx]\n", addr);
-//      uint32_t phys_addr = get_bank_addr((void *)addr);
-////    DEBUG_LOG(ARM11, "[PHYSICAL][ADDR:0x%08llx]\n", phys_addr);
-//      flush_bb(phys_addr);
-//      flush_bb(phys_addr + 4096);
-//#if HYBRID_MODE
-//      /* flush the translated BB of dyncom */
-//              clear_translated_cache(phys_addr); 
-//#endif
-//      #endif
-//}
-//#endif /* shenoubang */
-
-//void protect_code_page(uint32_t addr)
-//{
-//      void *mem_ptr = (void *)get_dma_addr(addr);
-//      mem_ptr = (void *)((long long int)mem_ptr & 0xfffffffffffff000LL);
-//
-//      const vector<uint64_t>::iterator it = find(code_page_set.begin(), 
-//                                                 code_page_set.end(),
-//                                                 (uint64_t)mem_ptr);
-//      if (it != code_page_set.end()) {
-//              return;
-//      }
-//      //DEBUG_LOG(ARM11, "[mprotect][ADDR:0x%08llx]\n", mem_ptr);
-//      /* shenoubang add win32 2012-6-12 */
-//#ifndef __WIN32__
-//      struct sigaction sa;
-//
-//      memset(&sa, 0, sizeof(sa));
-//      sa.sa_flags = SA_RESTART | SA_SIGINFO;
-//      sa.sa_sigaction = &flush_code_cache;
-//      sigaction(SIGSEGV, &sa, NULL);
-//
-//      //mprotect(mem_ptr, 4096, PROT_READ);
-//
-//      code_page_set.push_back((uint64_t)mem_ptr);
-//#endif /* shenoubang */
-//}
-
-
-
-int InterpreterTranslate(arm_processor *cpu, int &bb_start, addr_t addr)
-{
-        /* Decode instruction, get index */
-        /* Allocate memory and init InsCream */
-        /* Go on next, until terminal instruction */
-        /* Save start addr of basicblock in CreamCache */
-        //arm_processor *cpu = (arm_processor *)get_cast_conf_obj(core->cpu_data, "arm_core_t");
-        //arm_processor *cpu = (arm_processor *)(core->cpu_data->obj);
-        ARM_INST_PTR inst_base = NULL;
-        unsigned int inst, inst_size = 4;
-        int idx;
-        int ret = NON_BRANCH;
-        int thumb = 0;
-        /* instruction size of basic block */
-        int size = 0;
-        /* (R15 - 8) ? */
-        //cpu->translate_pc = cpu->Reg[15];
-        bb_start = top;
-
-        if (cpu->TFlag)
-                thumb = THUMB;
-
-        addr_t phys_addr;
-        addr_t pc_start;
-        fault_t fault = NO_FAULT;
-        //fault = check_address_validity(cpu, addr, &phys_addr, 1, INSN_TLB);
-        fault = check_address_validity(cpu, addr, &phys_addr, 1);
-        if(fault != NO_FAULT){
-                cpu->abortSig = true;
-                cpu->Aborted = ARMul_PrefetchAbortV;
-                cpu->AbortAddr = addr;
-                cpu->CP15[CP15(CP15_INSTR_FAULT_STATUS)] = fault & 0xff;
-                cpu->CP15[CP15(CP15_FAULT_ADDRESS)] = addr;
-                return FETCH_EXCEPTION;
-        }
-        pc_start = phys_addr;
-        //phys_addr = get_dma_addr(phys_addr);
-        while(ret == NON_BRANCH) {
-                /* shenoubang add win32 2012-6-14 */
-#ifdef __WIN32__
-               mem_bank_t* bank;
-               if (bank = bank_ptr(addr)) {
-                       bank->bank_read(32, phys_addr, &inst);
-               }
-               else {
-                       LOG_ERROR(Core_ARM11, "SKYEYE: Read physical addr 0x%x error!!\n", phys_addr);
-                       return FETCH_FAILURE;
-               }
-#else
-        inst = Memory::Read32(phys_addr & 0xFFFFFFFC);//*(uint32_t *)(phys_addr & 0xFFFFFFFC);
-#endif
-                //or_tag(core, phys_addr,  TAG_FAST_INTERP);
-
-                /*if (ret == FETCH_FAILURE) {
-                        return FETCH_EXCEPTION;
-                }*/
-
-                size ++;
-                /* If we are in thumb instruction, we will translate one thumb to one corresponding arm instruction */
-                if (cpu->TFlag){
-                //if(cpu->Cpsr & (1 << THUMB_BIT)){
-                        uint32_t arm_inst;
-                        tdstate state;
-                        state = decode_thumb_instr(cpu, inst, phys_addr, &arm_inst, &inst_size, &inst_base);
-                        //or_tag(core, phys_addr, TAG_THUMB);
-                        //DEBUG_LOG(ARM11, "In thumb state, arm_inst=0x%x, inst_size=0x%x, pc=0x%x\n", arm_inst, inst_size, cpu->translate_pc);
-                        /* we have translated the branch instruction of thumb in thumb decoder */
-                        if(state == t_branch){
-                                goto translated;
-                        }
-                        inst = arm_inst;
-                }
-
-                ret = decode_arm_instr(inst, &idx);
-                if (ret == DECODE_FAILURE) {
-                        LOG_ERROR(Core_ARM11, "Decode failure.\tPC : [0x%x]\tInstruction : [%x]", phys_addr, inst);
-                        LOG_ERROR(Core_ARM11, "cpsr=0x%x, cpu->TFlag=%d, r15=0x%x", cpu->Cpsr, cpu->TFlag, cpu->Reg[15]);
-                        CITRA_IGNORE_EXIT(-1);
-                }
-//              DEBUG_LOG(ARM11, "PC : [0x%x] INST : %s\n", cpu->translate_pc, arm_instruction[idx].name);
-                inst_base = arm_instruction_trans[idx](inst, idx);
-//              DEBUG_LOG(ARM11, "translated @ %x INST : %x\n", cpu->translate_pc, inst);
-//              DEBUG_LOG(ARM11, "inst size is %d\n", InstLength[idx]);
-translated:
-                phys_addr += inst_size;
+void flush_bb(uint32_t addr) {
+    bb_map::iterator it;
+    uint32_t start;
+
+    addr  &= 0xfffff000;
+    for (it = CreamCache.begin(); it != CreamCache.end(); ) {
+        start = static_cast<uint32_t>(it->first);
+        start &= 0xfffff000;
+        if (start == addr) {
+            CreamCache.erase(it++);
+        } else
+            ++it;
+    }
+}
 
-                if ((phys_addr & 0xfff) == 0) {
-                        inst_base->br = END_OF_PAGE;
-                }
-                ret = inst_base->br;
-        };
+int InterpreterTranslate(arm_processor *cpu, int &bb_start, addr_t addr) {
+    // Decode instruction, get index
+    // Allocate memory and init InsCream
+    // Go on next, until terminal instruction
+    // Save start addr of basicblock in CreamCache
+    ARM_INST_PTR inst_base = nullptr;
+    unsigned int inst, inst_size = 4;
+    int idx;
+    int ret = NON_BRANCH;
+    int thumb = 0;
+    int size = 0; // instruction size of basic block
+    bb_start = top;
+
+    if (cpu->TFlag)
+        thumb = THUMB;
+
+    addr_t phys_addr;
+    addr_t pc_start;
+    fault_t fault = NO_FAULT;
+    fault = check_address_validity(cpu, addr, &phys_addr, 1);
+    if(fault != NO_FAULT){
+        cpu->abortSig = true;
+        cpu->Aborted = ARMul_PrefetchAbortV;
+        cpu->AbortAddr = addr;
+        cpu->CP15[CP15(CP15_INSTR_FAULT_STATUS)] = fault & 0xff;
+        cpu->CP15[CP15(CP15_FAULT_ADDRESS)] = addr;
+        return FETCH_EXCEPTION;
+    }
 
-        //DEBUG_LOG(ARM11, "In %s,insert_bb pc=0x%x, TFlag=0x%x\n", __FUNCTION__, pc_start, cpu->TFlag);
-        insert_bb(pc_start, bb_start);
-        return KEEP_GOING;
-}
+    pc_start = phys_addr;
 
-#define LOG_IN_CLR      skyeye_printf_in_color
+    while(ret == NON_BRANCH) {
+        inst = Memory::Read32(phys_addr & 0xFFFFFFFC);//*(uint32_t *)(phys_addr & 0xFFFFFFFC);
 
-int cmp(const void *x, const void *y)
-{
-        return *(unsigned long long int*)x - *(unsigned long long int *)y;
+        size ++;
+        // If we are in thumb instruction, we will translate one thumb to one corresponding arm instruction
+        if (cpu->TFlag) {
+            uint32_t arm_inst;
+            tdstate state;
+            state = decode_thumb_instr(cpu, inst, phys_addr, &arm_inst, &inst_size, &inst_base);
+            // We have translated the branch instruction of thumb in thumb decoder
+            if(state == t_branch){
+                goto translated;
+            }
+            inst = arm_inst;
+        }
+
+        ret = decode_arm_instr(inst, &idx);
+        if (ret == DECODE_FAILURE) {
+            LOG_ERROR(Core_ARM11, "Decode failure.\tPC : [0x%x]\tInstruction : [%x]", phys_addr, inst);
+            LOG_ERROR(Core_ARM11, "cpsr=0x%x, cpu->TFlag=%d, r15=0x%x", cpu->Cpsr, cpu->TFlag, cpu->Reg[15]);
+            CITRA_IGNORE_EXIT(-1);
+        }
+        inst_base = arm_instruction_trans[idx](inst, idx);
+translated:
+        phys_addr += inst_size;
+
+        if ((phys_addr & 0xfff) == 0) {
+            inst_base->br = END_OF_PAGE;
+        }
+        ret = inst_base->br;
+    };
+    insert_bb(pc_start, bb_start);
+    return KEEP_GOING;
+}
+
+#define LOG_IN_CLR    skyeye_printf_in_color
+
+int cmp(const void *x, const void *y) {
+    return *(unsigned long long int*)x - *(unsigned long long int *)y;
+}
+
+void InterpreterInitInstLength(unsigned long long int *ptr, size_t size) {
+    int array_size = size / sizeof(void *);
+    unsigned long long int *InstLabel = new unsigned long long int[array_size];
+    memcpy(InstLabel, ptr, size);
+    qsort(InstLabel, array_size, sizeof(void *), cmp);
+    InstLength = new unsigned int[array_size - 4];
+    for (int i = 0; i < array_size - 4; i ++) {
+        for (int j = 0; j < array_size; j ++) {
+            if (ptr[i] == InstLabel[j]) {
+                InstLength[i] = InstLabel[j + 1] - InstLabel[j];
+                break;
+            }
+        }
+    }
+    for (int i = 0; i < array_size - 4; i ++)
+        LOG_DEBUG(Core_ARM11, "[%d]:%d", i, InstLength[i]);
 }
 
-void InterpreterInitInstLength(unsigned long long int *ptr, size_t size)
-{
-        int array_size = size / sizeof(void *);
-        unsigned long long int *InstLabel = new unsigned long long int[array_size];
-        memcpy(InstLabel, ptr, size);
-        qsort(InstLabel, array_size, sizeof(void *), cmp);
-        InstLength = new unsigned int[array_size - 4];
-        for (int i = 0; i < array_size - 4; i ++) {
-                for (int j = 0; j < array_size; j ++) {
-                        if (ptr[i] == InstLabel[j]) {
-                                InstLength[i] = InstLabel[j + 1] - InstLabel[j];
-                                break;
-                        }
-                }
-        }
-        for (int i = 0; i < array_size - 4; i ++)
-                LOG_DEBUG(Core_ARM11, "[%d]:%d", i, InstLength[i]);
-}
-
-int clz(unsigned int x)
-{
-        int n;
-        if (x == 0) return (32);
-        n = 1;
-        if ((x >> 16) == 0) { n = n + 16; x = x << 16;}
-        if ((x >> 24) == 0) { n = n +  8; x = x <<  8;}
-        if ((x >> 28) == 0) { n = n +  4; x = x <<  4;}
-        if ((x >> 30) == 0) { n = n +  2; x = x <<  2;}
-        n = n - (x >> 31);
-        return n;
+int clz(unsigned int x) {
+    int n;
+    if (x == 0) return (32);
+    n = 1;
+    if ((x >> 16) == 0) { n = n + 16; x = x << 16;}
+    if ((x >> 24) == 0) { n = n +  8; x = x <<  8;}
+    if ((x >> 28) == 0) { n = n +  4; x = x <<  4;}
+    if ((x >> 30) == 0) { n = n +  2; x = x <<  2;}
+    n = n - (x >> 31);
+    return n;
 }
 
 unsigned arm_dyncom_SWI (ARMul_State * state, ARMword number);
 
-static bool InAPrivilegedMode(arm_core_t *core)
-{
-        return (core->Mode != USER32MODE);
+static bool InAPrivilegedMode(arm_core_t *core) {
+    return (core->Mode != USER32MODE);
 }
 
-/* r15 = r15 + 8 */
-unsigned InterpreterMainLoop(ARMul_State* state)
-{
-        #define CRn                             inst_cream->crn
-        #define OPCODE_2                        inst_cream->opcode_2
-        #define CRm                             inst_cream->crm
-        #define CP15_REG(n)                     cpu->CP15[CP15(n)]
-        #define RD                              cpu->Reg[inst_cream->Rd]
-        #define RN                              cpu->Reg[inst_cream->Rn]
-        #define RM                              cpu->Reg[inst_cream->Rm]
-        #define RS                              cpu->Reg[inst_cream->Rs]
-        #define RDHI                            cpu->Reg[inst_cream->RdHi]
-        #define RDLO                            cpu->Reg[inst_cream->RdLo]
-        #define LINK_RTN_ADDR                   (cpu->Reg[14] = cpu->Reg[15] + 4)
-        #define SET_PC                          (cpu->Reg[15] = cpu->Reg[15] + 8 + inst_cream->signed_immed_24)
-        #define SHIFTER_OPERAND                 inst_cream->shtop_func(cpu, inst_cream->shifter_operand)
-
-        #if ENABLE_ICOUNTER
-        #define INC_ICOUNTER                    cpu->icounter++;                                                   \
-                                                if(cpu->Reg[15] > 0xc0000000)                                   \
-                                                        cpu->kernel_icounter++;
-                                                //if (debug_function(core))                                          \
-                                                        if (core->check_int_flag)                                  \
-                                                                goto END
-                                                //LOG_TRACE(Core_ARM11, "icounter is %llx pc is %x\n", cpu->icounter, cpu->Reg[15])
-        #else
-        #define INC_ICOUNTER                    ;                                                   
-        #endif
-
-        #define FETCH_INST                      if (inst_base->br != NON_BRANCH)                                   \
-                                                        goto DISPATCH;                                             \
-                                                inst_base = (arm_inst *)&inst_buf[ptr]                             
-#define INC_PC(l)                       ptr += sizeof(arm_inst) + l
+unsigned InterpreterMainLoop(ARMul_State* state) {
+    #define CRn             inst_cream->crn
+    #define OPCODE_2        inst_cream->opcode_2
+    #define CRm             inst_cream->crm
+    #define CP15_REG(n)     cpu->CP15[CP15(n)]
+    #define RD              cpu->Reg[inst_cream->Rd]
+    #define RN              cpu->Reg[inst_cream->Rn]
+    #define RM              cpu->Reg[inst_cream->Rm]
+    #define RS              cpu->Reg[inst_cream->Rs]
+    #define RDHI            cpu->Reg[inst_cream->RdHi]
+    #define RDLO            cpu->Reg[inst_cream->RdLo]
+    #define LINK_RTN_ADDR   (cpu->Reg[14] = cpu->Reg[15] + 4)
+    #define SET_PC          (cpu->Reg[15] = cpu->Reg[15] + 8 + inst_cream->signed_immed_24)
+    #define SHIFTER_OPERAND inst_cream->shtop_func(cpu, inst_cream->shifter_operand)
+
+    #define FETCH_INST if (inst_base->br != NON_BRANCH) goto DISPATCH; \
+                       inst_base = (arm_inst *)&inst_buf[ptr]
+
+    #define INC_PC(l) ptr += sizeof(arm_inst) + l
 
 // GCC and Clang have a C++ extension to support a lookup table of labels. Otherwise, fallback to a
 // clunky switch statement.
@@ -3872,7 +3738,7 @@ unsigned InterpreterMainLoop(ARMul_State* state)
     case 124: goto PKHTB_INST; \
     case 125: goto PKHBT_INST; \
     case 126: goto SMUL_INST; \
-    case 127: goto SMLAL_INST; \
+    case 127: goto SMLALXY_INST; \
     case 128: goto SMLA_INST; \
     case 129: goto MCRR_INST; \
     case 130: goto MRRC_INST; \
@@ -3941,2606 +3807,2522 @@ unsigned InterpreterMainLoop(ARMul_State* state)
     }
 #endif
 
-        #define UPDATE_NFLAG(dst)               (cpu->NFlag = BIT(dst, 31) ? 1 : 0)
-        #define UPDATE_ZFLAG(dst)               (cpu->ZFlag = dst ? 0 : 1)
-//      #define UPDATE_CFLAG(dst, lop, rop)     (cpu->CFlag = ((ISNEG(lop) && ISPOS(rop)) ||                        \
-                                                                (ISNEG(lop) && ISPOS(dst)) ||                       \
-                                                                (ISPOS(rop) && ISPOS(dst))))
-        #define UPDATE_CFLAG(dst, lop, rop)     (cpu->CFlag = ((dst < lop) || (dst < rop)))
-        #define UPDATE_CFLAG_CARRY_FROM_ADD(lop, rop, flag)     (cpu->CFlag = (((uint64_t) lop + (uint64_t) rop + (uint64_t) flag) > 0xffffffff) )
-        #define UPDATE_CFLAG_NOT_BORROW_FROM_FLAG(lop, rop, flag) (cpu->CFlag = ((uint64_t) lop >= ((uint64_t) rop + (uint64_t) flag)))
-        #define UPDATE_CFLAG_NOT_BORROW_FROM(lop, rop)  (cpu->CFlag = (lop >= rop))
-        #define UPDATE_CFLAG_WITH_NOT(dst, lop, rop)    (cpu->CFlag = !(dst < lop))
-        #define UPDATE_CFLAG_WITH_SC            cpu->CFlag = cpu->shifter_carry_out
-//      #define UPDATE_CFLAG_WITH_NOT(dst, lop, rop)    cpu->CFlag = !((ISNEG(lop) && ISPOS(rop)) ||                        \
-                                                                (ISNEG(lop) && ISPOS(dst)) ||                       \
-                                                                (ISPOS(rop) && ISPOS(dst)))
-        #define UPDATE_VFLAG(dst, lop, rop)     (cpu->VFlag = (((lop < 0) && (rop < 0) && (dst >= 0)) ||            \
-                                                                ((lop >= 0) && (rop) >= 0 && (dst < 0))))
-        #define UPDATE_VFLAG_WITH_NOT(dst, lop, rop)    (cpu->VFlag = !(((lop < 0) && (rop < 0) && (dst >= 0)) ||            \
-                                                                ((lop >= 0) && (rop) >= 0 && (dst < 0))))
-        #define UPDATE_VFLAG_OVERFLOW_FROM(dst, lop, rop)       (cpu->VFlag = (((lop ^ rop) & (lop ^ dst)) >> 31))
-
-        #define SAVE_NZCVT                      cpu->Cpsr = (cpu->Cpsr & 0x0fffffdf) | \
-                                                (cpu->NFlag << 31)   |                 \
-                                                (cpu->ZFlag << 30)   |                 \
-                                                (cpu->CFlag << 29)   |                 \
-                                                (cpu->VFlag << 28)   |                  \
-                                                (cpu->TFlag << 5)
-        #define LOAD_NZCVT                      cpu->NFlag = (cpu->Cpsr >> 31);   \
-                                                cpu->ZFlag = (cpu->Cpsr >> 30) & 1;   \
-                                                cpu->CFlag = (cpu->Cpsr >> 29) & 1;   \
-                                                cpu->VFlag = (cpu->Cpsr >> 28) & 1;     \
-                                                cpu->TFlag = (cpu->Cpsr >> 5) & 1;
-
-        #define CurrentModeHasSPSR              (cpu->Mode != SYSTEM32MODE) && (cpu->Mode != USER32MODE)
-        #define PC                              (cpu->Reg[15])
-        #define CHECK_EXT_INT                   if (!cpu->NirqSig) {                       \
-                                                        if (!(cpu->Cpsr & 0x80)) {         \
-                                                                goto END;                  \
-                                                        }                                  \
-                                                }
-
-        
-
-        //arm_processor *cpu = (arm_processor *)get_cast_conf_obj(core->cpu_data, "arm_core_t");
-        arm_processor *cpu = state; //(arm_processor *)(core->cpu_data->obj);
+    #define UPDATE_NFLAG(dst) (cpu->NFlag = BIT(dst, 31) ? 1 : 0)
+    #define UPDATE_ZFLAG(dst) (cpu->ZFlag = dst ? 0 : 1)
+
+    #define UPDATE_CFLAG(dst, lop, rop) (cpu->CFlag = ((dst < lop) || (dst < rop)))
+    #define UPDATE_CFLAG_CARRY_FROM_ADD(lop, rop, flag) (cpu->CFlag = (((uint64_t) lop + (uint64_t) rop + (uint64_t) flag) > 0xffffffff) )
+    #define UPDATE_CFLAG_NOT_BORROW_FROM_FLAG(lop, rop, flag) (cpu->CFlag = ((uint64_t) lop >= ((uint64_t) rop + (uint64_t) flag)))
+    #define UPDATE_CFLAG_NOT_BORROW_FROM(lop, rop)  (cpu->CFlag = (lop >= rop))
+    #define UPDATE_CFLAG_WITH_NOT(dst, lop, rop)    (cpu->CFlag = !(dst < lop))
+    #define UPDATE_CFLAG_WITH_SC                    (cpu->CFlag = cpu->shifter_carry_out)
+
+    #define UPDATE_VFLAG(dst, lop, rop) (cpu->VFlag = (((lop < 0) && (rop < 0) && (dst >= 0)) || \
+                                ((lop >= 0) && (rop) >= 0 && (dst < 0))))
+    #define UPDATE_VFLAG_WITH_NOT(dst, lop, rop)    (cpu->VFlag = !(((lop < 0) && (rop < 0) && (dst >= 0)) || \
+                                ((lop >= 0) && (rop) >= 0 && (dst < 0))))
+    #define UPDATE_VFLAG_OVERFLOW_FROM(dst, lop, rop) (cpu->VFlag = (((lop ^ rop) & (lop ^ dst)) >> 31))
+
+    #define SAVE_NZCVT cpu->Cpsr = (cpu->Cpsr & 0x0fffffdf) | \
+                      (cpu->NFlag << 31) | \
+                      (cpu->ZFlag << 30) | \
+                      (cpu->CFlag << 29) | \
+                      (cpu->VFlag << 28) | \
+                      (cpu->TFlag << 5)
+    #define LOAD_NZCVT cpu->NFlag = (cpu->Cpsr >> 31);     \
+                       cpu->ZFlag = (cpu->Cpsr >> 30) & 1; \
+                       cpu->CFlag = (cpu->Cpsr >> 29) & 1; \
+                       cpu->VFlag = (cpu->Cpsr >> 28) & 1; \
+                       cpu->TFlag = (cpu->Cpsr >> 5) & 1;
+
+    #define CurrentModeHasSPSR (cpu->Mode != SYSTEM32MODE) && (cpu->Mode != USER32MODE)
+    #define PC (cpu->Reg[15])
+    #define CHECK_EXT_INT if (!cpu->NirqSig && !(cpu->Cpsr & 0x80)) goto END;
+
+    arm_processor *cpu = state;
 
     // GCC and Clang have a C++ extension to support a lookup table of labels. Otherwise, fallback
     // to a clunky switch statement.
 #if defined __GNUC__ || defined __clang__
     void *InstLabel[] = {
-                #define VFP_INTERPRETER_LABEL
-                #include "core/arm/skyeye_common/vfp/vfpinstr.cpp"
-                #undef VFP_INTERPRETER_LABEL
-                &&SRS_INST,&&RFE_INST,&&BKPT_INST,&&BLX_INST,&&CPS_INST,&&PLD_INST,&&SETEND_INST,&&CLREX_INST,&&REV16_INST,&&USAD8_INST,&&SXTB_INST,
-                &&UXTB_INST,&&SXTH_INST,&&SXTB16_INST,&&UXTH_INST,&&UXTB16_INST,&&CPY_INST,&&UXTAB_INST,&&SSUB8_INST,&&SHSUB8_INST,&&SSUBADDX_INST,
-                &&STREX_INST,&&STREXB_INST,&&SWP_INST,&&SWPB_INST,&&SSUB16_INST,&&SSAT16_INST,&&SHSUBADDX_INST,&&QSUBADDX_INST,&&SHADDSUBX_INST,
-                &&SHADD8_INST,&&SHADD16_INST,&&SEL_INST,&&SADDSUBX_INST,&&SADD8_INST,&&SADD16_INST,&&SHSUB16_INST,&&UMAAL_INST,&&UXTAB16_INST,
-                &&USUBADDX_INST,&&USUB8_INST,&&USUB16_INST,&&USAT16_INST,&&USADA8_INST,&&UQSUBADDX_INST,&&UQSUB8_INST,&&UQSUB16_INST,
-                &&UQADDSUBX_INST,&&UQADD8_INST,&&UQADD16_INST,&&SXTAB_INST,&&UHSUBADDX_INST,&&UHSUB8_INST,&&UHSUB16_INST,&&UHADDSUBX_INST,&&UHADD8_INST,
-                &&UHADD16_INST,&&UADDSUBX_INST,&&UADD8_INST,&&UADD16_INST,&&SXTAH_INST,&&SXTAB16_INST,&&QADD8_INST,&&BXJ_INST,&&CLZ_INST,&&UXTAH_INST,
-                &&BX_INST,&&REV_INST,&&BLX_INST,&&REVSH_INST,&&QADD_INST,&&QADD16_INST,&&QADDSUBX_INST,&&LDREX_INST,&&QDADD_INST,&&QDSUB_INST,
-                &&QSUB_INST,&&LDREXB_INST,&&QSUB8_INST,&&QSUB16_INST,&&SMUAD_INST,&&SMMUL_INST,&&SMUSD_INST,&&SMLSD_INST,&&SMLSLD_INST,&&SMMLA_INST,
-                &&SMMLS_INST,&&SMLALD_INST,&&SMLAD_INST,&&SMLAW_INST,&&SMULW_INST,&&PKHTB_INST,&&PKHBT_INST,&&SMUL_INST,&&SMLAL_INST,&&SMLA_INST,
-                &&MCRR_INST,&&MRRC_INST,&&CMP_INST,&&TST_INST,&&TEQ_INST,&&CMN_INST,&&SMULL_INST,&&UMULL_INST,&&UMLAL_INST,&&SMLAL_INST,&&MUL_INST,
-                &&MLA_INST,&&SSAT_INST,&&USAT_INST,&&MRS_INST,&&MSR_INST,&&AND_INST,&&BIC_INST,&&LDM_INST,&&EOR_INST,&&ADD_INST,&&RSB_INST,&&RSC_INST,
-                &&SBC_INST,&&ADC_INST,&&SUB_INST,&&ORR_INST,&&MVN_INST,&&MOV_INST,&&STM_INST,&&LDM_INST,&&LDRSH_INST,&&STM_INST,&&LDM_INST,&&LDRSB_INST,
-                &&STRD_INST,&&LDRH_INST,&&STRH_INST,&&LDRD_INST,&&STRT_INST,&&STRBT_INST,&&LDRBT_INST,&&LDRT_INST,&&MRC_INST,&&MCR_INST,&&MSR_INST,
-                &&LDRB_INST,&&STRB_INST,&&LDR_INST,&&LDRCOND_INST, &&STR_INST,&&CDP_INST,&&STC_INST,&&LDC_INST,&&SWI_INST,&&BBL_INST,&&B_2_THUMB, &&B_COND_THUMB, 
-                &&BL_1_THUMB, &&BL_2_THUMB, &&BLX_1_THUMB, &&DISPATCH,&&INIT_INST_LENGTH,&&END
-                };
-#endif
-        arm_inst * inst_base;
-        unsigned int lop, rop, dst;
-        unsigned int addr;
-        unsigned int phys_addr;
-        unsigned int last_pc = 0;
-        unsigned int num_instrs = 0;
-        fault_t fault;
-        static unsigned int last_physical_base = 0, last_logical_base = 0;
-        int ptr;
-        bool single_step = (cpu->NumInstrsToExecute == 1);
-
-        LOAD_NZCVT;
-        DISPATCH:
-        {
-                if (!cpu->NirqSig) {
-                        if (!(cpu->Cpsr & 0x80)) {
-                                goto END;
-                        }
-                }
-
-                if (cpu->TFlag) {
-                        cpu->Reg[15] &= 0xfffffffe;
-                } else
-                        cpu->Reg[15] &= 0xfffffffc;
-#if PROFILE
-                /* check next instruction address is valid. */
-                last_pc = cpu->Reg[15];
+        &&VMLA_INST, &&VMLS_INST, &&VNMLA_INST, &&VNMLA_INST, &&VNMLS_INST, &&VNMUL_INST, &&VMUL_INST, &&VADD_INST, &&VSUB_INST,
+        &&VDIV_INST, &&VMOVI_INST, &&VMOVR_INST, &&VABS_INST, &&VNEG_INST, &&VSQRT_INST, &&VCMP_INST, &&VCMP2_INST, &&VCVTBDS_INST,
+        &&VCVTBFF_INST, &&VCVTBFI_INST, &&VMOVBRS_INST, &&VMSR_INST, &&VMOVBRC_INST, &&VMRS_INST, &&VMOVBCR_INST, &&VMOVBRRSS_INST,
+        &&VMOVBRRD_INST, &&VSTR_INST, &&VPUSH_INST, &&VSTM_INST, &&VPOP_INST, &&VLDR_INST, &&VLDM_INST,
+
+        &&SRS_INST,&&RFE_INST,&&BKPT_INST,&&BLX_INST,&&CPS_INST,&&PLD_INST,&&SETEND_INST,&&CLREX_INST,&&REV16_INST,&&USAD8_INST,&&SXTB_INST,
+        &&UXTB_INST,&&SXTH_INST,&&SXTB16_INST,&&UXTH_INST,&&UXTB16_INST,&&CPY_INST,&&UXTAB_INST,&&SSUB8_INST,&&SHSUB8_INST,&&SSUBADDX_INST,
+        &&STREX_INST,&&STREXB_INST,&&SWP_INST,&&SWPB_INST,&&SSUB16_INST,&&SSAT16_INST,&&SHSUBADDX_INST,&&QSUBADDX_INST,&&SHADDSUBX_INST,
+        &&SHADD8_INST,&&SHADD16_INST,&&SEL_INST,&&SADDSUBX_INST,&&SADD8_INST,&&SADD16_INST,&&SHSUB16_INST,&&UMAAL_INST,&&UXTAB16_INST,
+        &&USUBADDX_INST,&&USUB8_INST,&&USUB16_INST,&&USAT16_INST,&&USADA8_INST,&&UQSUBADDX_INST,&&UQSUB8_INST,&&UQSUB16_INST,
+        &&UQADDSUBX_INST,&&UQADD8_INST,&&UQADD16_INST,&&SXTAB_INST,&&UHSUBADDX_INST,&&UHSUB8_INST,&&UHSUB16_INST,&&UHADDSUBX_INST,&&UHADD8_INST,
+        &&UHADD16_INST,&&UADDSUBX_INST,&&UADD8_INST,&&UADD16_INST,&&SXTAH_INST,&&SXTAB16_INST,&&QADD8_INST,&&BXJ_INST,&&CLZ_INST,&&UXTAH_INST,
+        &&BX_INST,&&REV_INST,&&BLX_INST,&&REVSH_INST,&&QADD_INST,&&QADD16_INST,&&QADDSUBX_INST,&&LDREX_INST,&&QDADD_INST,&&QDSUB_INST,
+        &&QSUB_INST,&&LDREXB_INST,&&QSUB8_INST,&&QSUB16_INST,&&SMUAD_INST,&&SMMUL_INST,&&SMUSD_INST,&&SMLSD_INST,&&SMLSLD_INST,&&SMMLA_INST,
+        &&SMMLS_INST,&&SMLALD_INST,&&SMLAD_INST,&&SMLAW_INST,&&SMULW_INST,&&PKHTB_INST,&&PKHBT_INST,&&SMUL_INST,&&SMLALXY_INST,&&SMLA_INST,
+        &&MCRR_INST,&&MRRC_INST,&&CMP_INST,&&TST_INST,&&TEQ_INST,&&CMN_INST,&&SMULL_INST,&&UMULL_INST,&&UMLAL_INST,&&SMLAL_INST,&&MUL_INST,
+        &&MLA_INST,&&SSAT_INST,&&USAT_INST,&&MRS_INST,&&MSR_INST,&&AND_INST,&&BIC_INST,&&LDM_INST,&&EOR_INST,&&ADD_INST,&&RSB_INST,&&RSC_INST,
+        &&SBC_INST,&&ADC_INST,&&SUB_INST,&&ORR_INST,&&MVN_INST,&&MOV_INST,&&STM_INST,&&LDM_INST,&&LDRSH_INST,&&STM_INST,&&LDM_INST,&&LDRSB_INST,
+        &&STRD_INST,&&LDRH_INST,&&STRH_INST,&&LDRD_INST,&&STRT_INST,&&STRBT_INST,&&LDRBT_INST,&&LDRT_INST,&&MRC_INST,&&MCR_INST,&&MSR_INST,
+        &&LDRB_INST,&&STRB_INST,&&LDR_INST,&&LDRCOND_INST, &&STR_INST,&&CDP_INST,&&STC_INST,&&LDC_INST,&&SWI_INST,&&BBL_INST,&&B_2_THUMB, &&B_COND_THUMB, 
+        &&BL_1_THUMB, &&BL_2_THUMB, &&BLX_1_THUMB, &&DISPATCH,&&INIT_INST_LENGTH,&&END
+        };
 #endif
-#if USER_MODE_OPT
-                phys_addr = cpu->Reg[15];
-#else
-                {
-                        if (last_logical_base == (cpu->Reg[15] & 0xfffff000))
-                               phys_addr = last_physical_base + (cpu->Reg[15] & 0xfff);
-                        else {
-                               /* check next instruction address is valid. */
-                               fault = check_address_validity(cpu, cpu->Reg[15], &phys_addr, 1, INSN_TLB);
-                               if (fault) {
-                                       cpu->abortSig = true;
-                                       cpu->Aborted = ARMul_PrefetchAbortV;
-                                       cpu->AbortAddr = cpu->Reg[15];
-                                       cpu->CP15[CP15(CP15_INSTR_FAULT_STATUS)] = fault & 0xff;
-                                       cpu->CP15[CP15(CP15_FAULT_ADDRESS)] = cpu->Reg[15];
-                                       goto END;
-                               }
-                               last_logical_base = cpu->Reg[15] & 0xfffff000;
-                               last_physical_base = phys_addr & 0xfffff000;
-                        }
-                }
-#if HYBRID_MODE
-                /* check if the native code of dyncom is available */
-                //fast_map hash_map = core->dyncom_engine->fmap;
-                //void * pfunc = NULL;
-                //PFUNC(phys_addr);
-                //if(pfunc){
-                if(is_translated_entry(core, phys_addr)){
-                        int rc = JIT_RETURN_NOERR;
-                        //DEBUG_LOG(ARM11, "enter jit icounter is %lld, pc=0x%x\n", core->icounter, cpu->Reg[15]);
-                        SAVE_NZCVT;
-//                      resume_timing();
-                        rc = cpu_run(core);
-                        LOAD_NZCVT;
-                        //DEBUG_LOG(ARM11, "out of jit ret is %d icounter is %lld, pc=0x%x\n", rc, core->icounter, cpu->Reg[15]);
-                        if((rc == JIT_RETURN_FUNCNOTFOUND) || (rc == JIT_RETURN_FUNC_BLANK)){
-                                /* keep the tflag same with the bit in CPSR */
-                                //cpu->TFlag = cpu->Cpsr & (1 << THUMB_BIT);
-                                //cpu->TFlag = cpu->Cpsr & (1 << 5);
-                                //switch_mode(cpu, cpu->Cpsr & 0x1f);
-                                //DEBUG_LOG(ARM11, "FUNCTION not found , pc=0x%x\n", cpu->Reg[15]);
-                               fault = check_address_validity(cpu, cpu->Reg[15], &phys_addr, 1, INSN_TLB);
-                               if (fault) {
-                                       cpu->abortSig = true;
-                                       cpu->Aborted = ARMul_PrefetchAbortV;
-                                       cpu->AbortAddr = cpu->Reg[15];
-                                       cpu->CP15[CP15(CP15_INSTR_FAULT_STATUS)] = fault & 0xff;
-                                       cpu->CP15[CP15(CP15_FAULT_ADDRESS)] = cpu->Reg[15];
-                                       goto END;
-                                }
-                                last_logical_base = cpu->Reg[15] & 0xfffff000;
-                                last_physical_base = phys_addr & 0xfffff000;
-                                core->current_page_phys = last_physical_base;
-                                core->current_page_effec = last_logical_base;
-                                //push_to_compiled(core, phys_addr);
-                        }
-                        else{
-                                if((cpu->CP15[CP15(CP15_TLB_FAULT_STATUS)] & 0xf0)){
-                                        //DEBUG_LOG(ARM11, "\n\n###############In %s, fsr=0x%x, fault_addr=0x%x, pc=0x%x\n\n", __FUNCTION__, cpu->CP15[CP15(CP15_FAULT_STATUS)], cpu->CP15[CP15(CP15_FAULT_ADDRESS)], cpu->Reg[15]);
-                        //core->Reg[15] -= get_instr_size(cpu_dyncom);
-                                        fill_tlb(cpu);
-                                        goto END;
-                                }
-                                if (cpu->syscallSig) {
-                                        goto END;
-                                }
-                                if (cpu->abortSig) {
-                                        cpu->CP15[CP15_TLB_FAULT_STATUS - CP15_BASE] &= 0xFFFFFFF0;
-                                        goto END;
-                                }
-                                if (!cpu->NirqSig) {
-                                        if (!(cpu->Cpsr & 0x80)) {
-                                                goto END;
-                                        }
-                                }
-                        
-                                /* if regular trap */
-                                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                                /*uint32_t mode = cpu->Cpsr & 0x1f;
-                                if ((mode != cpu->Mode) && (!is_user_mode(core))) {
-                                        switch_mode(cpu, mode);
-                                        return 1;
-                                }*/
-
-                                goto END;
-                        }
-                        //phys_addr = cpu->Reg[15];
-                }
-                else{
-                        if (last_logical_base == (cpu->Reg[15] & 0xfffff000))
-                               phys_addr = last_physical_base + (cpu->Reg[15] & 0xfff);
-                        else {
-                               /* check next instruction address is valid. */
-                               fault = check_address_validity(cpu, cpu->Reg[15], &phys_addr, 1, INSN_TLB);
-                               if (fault) {
-                                       cpu->abortSig = true;
-                                       cpu->Aborted = ARMul_PrefetchAbortV;
-                                       cpu->AbortAddr = cpu->Reg[15];
-                                       cpu->CP15[CP15(CP15_INSTR_FAULT_STATUS)] = fault & 0xff;
-                                       cpu->CP15[CP15(CP15_FAULT_ADDRESS)] = cpu->Reg[15];
-                                       goto END;
-                               }
-                               last_logical_base = cpu->Reg[15] & 0xfffff000;
-                               last_physical_base = phys_addr & 0xfffff000;
-                        }
-                }
-#endif /* #if HYBRID_MODE */
-#endif /* #if USER_MODE_OPT */
-                if (true){//if(is_fast_interp_code(core, phys_addr)){
-                        if (find_bb(phys_addr, ptr) == -1)
-                                if (InterpreterTranslate(cpu, ptr, cpu->Reg[15]) == FETCH_EXCEPTION)
-                                        goto END;
-                }
-                else{
+    arm_inst * inst_base;
+    unsigned int lop, rop, dst;
+    unsigned int addr;
+    unsigned int phys_addr;
+    unsigned int last_pc = 0;
+    unsigned int num_instrs = 0;
+    fault_t fault;
+    static unsigned int last_physical_base = 0, last_logical_base = 0;
+    int ptr;
+    bool single_step = (cpu->NumInstrsToExecute == 1);
+
+    LOAD_NZCVT;
+    DISPATCH:
+    {
+        if (!cpu->NirqSig) {
+            if (!(cpu->Cpsr & 0x80)) {
+                goto END;
+            }
+        }
+
+        if (cpu->TFlag)
+            cpu->Reg[15] &= 0xfffffffe;
+        else
+            cpu->Reg[15] &= 0xfffffffc;
+
+        phys_addr = cpu->Reg[15];
+
+        if (find_bb(phys_addr, ptr) == -1)
             if (InterpreterTranslate(cpu, ptr, cpu->Reg[15]) == FETCH_EXCEPTION)
-                                goto END;
-                }
-#if PROFILE
-                resume_timing();
-#endif
-                inst_base = (arm_inst *)&inst_buf[ptr];
-                GOTO_NEXT_INST;
-        }
-        ADC_INST:
-        {
-                INC_ICOUNTER;
-                adc_inst *inst_cream = (adc_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        lop = RN;
-                        unsigned int sht_op = SHIFTER_OPERAND;
-                        rop = SHIFTER_OPERAND + cpu->CFlag;
-                        RD = dst = lop + rop;
-                        if (inst_cream->S && (inst_cream->Rd == 15)) {
-                                /* cpsr = spsr */
-                                if (CurrentModeHasSPSR) {
-                                        cpu->Cpsr = cpu->Spsr_copy;
-                                        switch_mode(cpu, cpu->Spsr_copy & 0x1f);
-                                        LOAD_NZCVT;
-                                }
-                        } else if (inst_cream->S) {
-                                UPDATE_NFLAG(dst);
-                                UPDATE_ZFLAG(dst);
-                                UPDATE_CFLAG_CARRY_FROM_ADD(lop, sht_op, cpu->CFlag);
-                                UPDATE_VFLAG((int)dst, (int)lop, (int)rop);
-                        }
-                        if (inst_cream->Rd == 15) {
-                                INC_PC(sizeof(adc_inst));
-                                goto DISPATCH;
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(adc_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        ADD_INST:
-        {
-                INC_ICOUNTER;
-                add_inst *inst_cream = (add_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        lop = RN;
-                        if (inst_cream->Rn == 15) {
-                                lop += 2 * GET_INST_SIZE(cpu);
-                        }
-                        rop = SHIFTER_OPERAND;
-                        RD = dst = lop + rop;
-                        if (inst_cream->S && (inst_cream->Rd == 15)) {
-                                /* cpsr = spsr*/
-                                if (CurrentModeHasSPSR) {
-                                        cpu->Cpsr = cpu->Spsr_copy;
-                                        switch_mode(cpu, cpu->Cpsr & 0x1f);
-                                        LOAD_NZCVT;
-                                }
-                        } else if (inst_cream->S) {
-                                UPDATE_NFLAG(dst);
-                                UPDATE_ZFLAG(dst);
-                                UPDATE_CFLAG(dst, lop, rop);
-                                UPDATE_VFLAG((int)dst, (int)lop, (int)rop);
-                        }
-                        if (inst_cream->Rd == 15) {
-                                INC_PC(sizeof(add_inst));
-                                goto DISPATCH;
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(add_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        AND_INST:
-        {
-                INC_ICOUNTER;
-                and_inst *inst_cream = (and_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        lop = RN;
-                        rop = SHIFTER_OPERAND;
-                        RD = dst = lop & rop;
-                        if (inst_cream->S && (inst_cream->Rd == 15)) {
-                                /* cpsr = spsr*/
-                                if (CurrentModeHasSPSR) {
-                                        cpu->Cpsr = cpu->Spsr_copy;
-                                        switch_mode(cpu, cpu->Cpsr & 0x1f);
-                                        LOAD_NZCVT;
-                                }
-                        } else if (inst_cream->S) {
-                                UPDATE_NFLAG(dst);
-                                UPDATE_ZFLAG(dst);
-                                UPDATE_CFLAG_WITH_SC;
-                                //UPDATE_VFLAG((int)dst, (int)lop, (int)rop);
-                        }
-                        if (inst_cream->Rd == 15) {
-                                INC_PC(sizeof(and_inst));
-                                goto DISPATCH;
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(and_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        BBL_INST:
-        {
-                INC_ICOUNTER;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        bbl_inst *inst_cream = (bbl_inst *)inst_base->component;
-                        if (inst_cream->L) {
-                                LINK_RTN_ADDR;
-                        }
-                        SET_PC;
-                        INC_PC(sizeof(bbl_inst));
-                        goto DISPATCH;
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(bbl_inst));
-                goto DISPATCH;
-        }
-        BIC_INST:
-        {
-                INC_ICOUNTER;
-                bic_inst *inst_cream = (bic_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        lop = RN;
-                        if (inst_cream->Rn == 15) {
-                                lop += 2 * GET_INST_SIZE(cpu);
-                        }
-                        rop = SHIFTER_OPERAND;
-//                      RD = dst = lop & (rop ^ 0xffffffff);
-                        RD = dst = lop & (~rop);
-                        if ((inst_cream->S) && (inst_cream->Rd == 15)) {
-                                /* cpsr = spsr */
-                                if (CurrentModeHasSPSR) {
-                                        cpu->Cpsr = cpu->Spsr_copy;
-                                        switch_mode(cpu, cpu->Spsr_copy & 0x1f);
-                                        LOAD_NZCVT;
-                                }
-                        } else if (inst_cream->S) {
-                                UPDATE_NFLAG(dst);
-                                UPDATE_ZFLAG(dst);
-                                UPDATE_CFLAG_WITH_SC;
-                        }
-                        if (inst_cream->Rd == 15) {
-                                INC_PC(sizeof(bic_inst));
-                                goto DISPATCH;
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(bic_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        BKPT_INST:
-        BLX_INST:
-        {
-                INC_ICOUNTER;
-                blx_inst *inst_cream = (blx_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        unsigned int inst = inst_cream->inst;
-                        if (BITS(inst, 20, 27) == 0x12 && BITS(inst, 4, 7) == 0x3) {
-                                //LINK_RTN_ADDR;
-                                cpu->Reg[14] = (cpu->Reg[15] + GET_INST_SIZE(cpu));
-                                if(cpu->TFlag)
-                                        cpu->Reg[14] |= 0x1;
-                                cpu->Reg[15] = cpu->Reg[inst_cream->val.Rm] & 0xfffffffe;
-                                cpu->TFlag = cpu->Reg[inst_cream->val.Rm] & 0x1;
-                                //cpu->Reg[15] = cpu->Reg[BITS(inst, 0, 3)] & 0xfffffffe;
-                                //cpu->TFlag = cpu->Reg[BITS(inst, 0, 3)] & 0x1;
-                        } else {
-                                cpu->Reg[14] = (cpu->Reg[15] + GET_INST_SIZE(cpu));
-                                cpu->TFlag = 0x1;
-                                int signed_int = inst_cream->val.signed_immed_24;
-                                signed_int = (signed_int) & 0x800000 ? (0x3F000000 | signed_int) : signed_int;
-                                signed_int = signed_int << 2;
-                        //      cpu->Reg[15] = cpu->Reg[15] + 2 * GET_INST_SIZE(cpu) 
-                                cpu->Reg[15] = cpu->Reg[15] + 8 
-                                                + signed_int + (BIT(inst, 24) << 1);
-                                //DEBUG_MSG;
-                        }
-                        INC_PC(sizeof(blx_inst));
-                        goto DISPATCH;
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-//              INC_PC(sizeof(bx_inst));
-                INC_PC(sizeof(blx_inst));
-                goto DISPATCH;
-        }
-        BX_INST:
-        {
-                INC_ICOUNTER;
-                bx_inst *inst_cream = (bx_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        if (inst_cream->Rm == 15)
-                                LOG_WARNING(Core_ARM11, "BX at pc %x: use of Rm = R15 is discouraged", cpu->Reg[15]);
-                        cpu->TFlag = cpu->Reg[inst_cream->Rm] & 0x1;
-                        cpu->Reg[15] = cpu->Reg[inst_cream->Rm] & 0xfffffffe;
-//                      cpu->TFlag = cpu->Reg[inst_cream->Rm] & 0x1;
-                        INC_PC(sizeof(bx_inst));
-                        goto DISPATCH;
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-//              INC_PC(sizeof(bx_inst));
-                INC_PC(sizeof(bx_inst));
-                goto DISPATCH;
-        }
-        BXJ_INST:
-        CDP_INST:
-        {
-                INC_ICOUNTER;
-                cdp_inst *inst_cream = (cdp_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        /* FIXME, check if cp access allowed */
-                        #define CP_ACCESS_ALLOW 0
-                        if(CP_ACCESS_ALLOW){
-                                /* undefined instruction here */
-                                cpu->NumInstrsToExecute = 0;
-                                return num_instrs;
-                        }
-                        LOG_ERROR(Core_ARM11, "CDP insn inst=0x%x, pc=0x%x\n", inst_cream->inst, cpu->Reg[15]);
-                        unsigned cpab = (cpu->CDP[inst_cream->cp_num]) (cpu, ARMul_FIRST, inst_cream->inst);
-                        if(cpab != ARMul_DONE){
-                                LOG_ERROR(Core_ARM11, "CDP insn wrong, inst=0x%x, cp_num=0x%x\n", inst_cream->inst, inst_cream->cp_num);
-                                //CITRA_IGNORE_EXIT(-1);
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(cdp_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-
-        CLREX_INST:
-        {
-                INC_ICOUNTER;
-                remove_exclusive(cpu, 0);
-                cpu->exclusive_state = 0;
-
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(clrex_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        CLZ_INST:
-        {
-                INC_ICOUNTER;
-                clz_inst *inst_cream = (clz_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        RD = clz(RM);
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(clz_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        CMN_INST:
-        {
-                INC_ICOUNTER;
-                cmn_inst *inst_cream = (cmn_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-//                      DEBUG_LOG(ARM11, "RN is %x\n", RN);
-                        lop = RN;
-                        rop = SHIFTER_OPERAND;
-                        dst = lop + rop;
-                        UPDATE_NFLAG(dst);
-                        UPDATE_ZFLAG(dst);
-                        UPDATE_CFLAG(dst, lop, rop);
-                        UPDATE_VFLAG((int)dst, (int)lop, (int)rop);
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(cmn_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        CMP_INST:
-        {
-//              DEBUG_LOG(ARM11, "cmp inst\n");
-//              DEBUG_LOG(ARM11, "pc:       %x\n", cpu->Reg[15]);
-                INC_ICOUNTER;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-//                      DEBUG_LOG(ARM11, "r0 is %x\n", cpu->Reg[0]);
-                        cmp_inst *inst_cream = (cmp_inst *)inst_base->component;
-                        lop = RN;
-                        if (inst_cream->Rn == 15) {
-                                lop += 2 * GET_INST_SIZE(cpu);
-                        }
-                        rop = SHIFTER_OPERAND;
-                        dst = lop - rop;
-
-                        UPDATE_NFLAG(dst);
-                        UPDATE_ZFLAG(dst);
-//                      UPDATE_CFLAG(dst, lop, rop);
-                        UPDATE_CFLAG_NOT_BORROW_FROM(lop, rop);
-//                      UPDATE_VFLAG((int)dst, (int)lop, (int)rop);
-                        UPDATE_VFLAG_OVERFLOW_FROM(dst, lop, rop);
-//                      UPDATE_VFLAG_WITH_NOT(dst, lop, rop);
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(cmp_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        CPS_INST:
-        {
-                INC_ICOUNTER;
-                cps_inst *inst_cream = (cps_inst *)inst_base->component;
-                uint32_t aif_val = 0;
-                uint32_t aif_mask = 0;
-                if (InAPrivilegedMode(cpu)) {
-                        /* isInAPrivilegedMode */
-                        if (inst_cream->imod1) {
-                                if (inst_cream->A) {
-                                        aif_val |= (inst_cream->imod0 << 8);
-                                        aif_mask |= 1 << 8;
-                                }
-                                if (inst_cream->I) {
-                                        aif_val |= (inst_cream->imod0 << 7);
-                                        aif_mask |= 1 << 7;
-                                }
-                                if (inst_cream->F) {
-                                        aif_val |= (inst_cream->imod0 << 6);
-                                        aif_mask |= 1 << 6;
-                                }
-                                aif_mask = ~aif_mask;
-                                cpu->Cpsr = (cpu->Cpsr & aif_mask) | aif_val;
-                        }
-                        if (inst_cream->mmod) {
-                                cpu->Cpsr = (cpu->Cpsr & 0xffffffe0) | inst_cream->mode;
-                                switch_mode(cpu, inst_cream->mode);
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(cps_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        CPY_INST:
-        {
-                INC_ICOUNTER;
-                mov_inst *inst_cream = (mov_inst *)inst_base->component;
-//              cpy_inst *inst_cream = (cpy_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        RD = SHIFTER_OPERAND;
-//                      RD = RM;
-                        if ((inst_cream->Rd == 15)) {
-                                INC_PC(sizeof(mov_inst));
-                                goto DISPATCH;
-                        }
-                }
-//              DEBUG_LOG(ARM11, "cpy inst %x\n", cpu->Reg[15]);
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(mov_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        EOR_INST:
-        {
-                INC_ICOUNTER;
-                eor_inst *inst_cream = (eor_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        lop = RN;
-                        if (inst_cream->Rn == 15) {
-                                lop += 2 * GET_INST_SIZE(cpu);
-                        }
-                        rop = SHIFTER_OPERAND;
-                        RD = dst = lop ^ rop;
-                        if (inst_cream->S && (inst_cream->Rd == 15)) {
-                                /* cpsr = spsr*/
-                                if (CurrentModeHasSPSR) {
-                                        cpu->Cpsr = cpu->Spsr_copy;
-                                        switch_mode(cpu, cpu->Spsr_copy & 0x1f);
-                                        LOAD_NZCVT;
-                                }
-                        } else if (inst_cream->S) {
-                                UPDATE_NFLAG(dst);
-                                UPDATE_ZFLAG(dst);
-                                UPDATE_CFLAG_WITH_SC;
-//                              UPDATE_CFLAG(dst, lop, rop);
-//                              UPDATE_VFLAG((int)dst, (int)lop, (int)rop);
-                        }
-                        if (inst_cream->Rd == 15) {
-                                INC_PC(sizeof(eor_inst));
-                                goto DISPATCH;
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(eor_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        LDC_INST:
-        {
-                INC_ICOUNTER;
-                /* NOT IMPL */
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(ldc_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        LDM_INST:
-        {
-                INC_ICOUNTER;
-                ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        int i;
-                        unsigned int ret;
-                        fault = inst_cream->get_addr(cpu, inst_cream->inst, addr, phys_addr, 1);
-                        if (fault) {
-                                goto MMU_EXCEPTION;
-                        }
-                        unsigned int inst = inst_cream->inst;
-                        if (BIT(inst, 22) && !BIT(inst, 15)) {
-//                              DEBUG_MSG;
-                                #if 1
-                                /* LDM (2) user */
-                                for (i = 0; i < 13; i++) {
-                                        if(BIT(inst, i)){
-                                                #if 0
-                                                fault = check_address_validity(cpu, addr, &phys_addr, 1);
-                                                if (fault) {
-                                                        goto MMU_EXCEPTION;
-                                                }
-                                                #endif
-                                                fault = interpreter_read_memory(addr, phys_addr, ret, 32);
-                                                //if (fault) goto MMU_EXCEPTION;
-                                                cpu->Reg[i] = ret;
-                                                addr += 4;
-                                                if ((addr & 0xfff) == 0) {
-                                                        fault = check_address_validity(cpu, addr, &phys_addr, 1);
-                                                } else {
-                                                        phys_addr += 4;
-                                                }
-                                        }
-                                }
-                                if (BIT(inst, 13)) {
-                                        #if 0
-                                        fault = check_address_validity(cpu, addr, &phys_addr, 1);
-                                        if (fault) {
-                                                goto MMU_EXCEPTION;
-                                        }
-                                        #endif
-                                        fault = interpreter_read_memory(addr, phys_addr, ret, 32);
-                                        //if (fault) goto MMU_EXCEPTION;
-                                        if (cpu->Mode == USER32MODE) 
-                                                cpu->Reg[13] = ret;
-                                        else
-                                                cpu->Reg_usr[0] = ret;
-                                        addr += 4;
-                                        if ((addr & 0xfff) == 0) {
-                                                fault = check_address_validity(cpu, addr, &phys_addr, 1);
-                                        } else {
-                                                phys_addr += 4;
-                                        }
-                                }
-                                if (BIT(inst, 14)) {
-                                        #if 0
-                                        fault = check_address_validity(cpu, addr, &phys_addr, 1);
-                                        if (fault) {
-                                                goto MMU_EXCEPTION;
-                                        }
-                                        #endif
-                                        fault = interpreter_read_memory(addr, phys_addr, ret, 32);
-                                        //if (fault) goto MMU_EXCEPTION;
-                                        if (cpu->Mode == USER32MODE) 
-                                                cpu->Reg[14] = ret;
-                                        else
-                                                cpu->Reg_usr[1] = ret;
-                                }
-                                #endif
-                        } else if (!BIT(inst, 22)) {
-                                for( i = 0; i < 16; i ++ ){
-                                        if(BIT(inst, i)){
-                                                //bus_read(32, addr, &ret);
-                                                #if 0
-                                                fault = check_address_validity(cpu, addr, &phys_addr, 1);
-                                                if (fault) {
-                                                        goto MMU_EXCEPTION;
-                                                }
-                                                #endif
-                                                fault = interpreter_read_memory(addr, phys_addr, ret, 32);
-                                                if (fault) goto MMU_EXCEPTION;
-                                                /* For armv5t, should enter thumb when bits[0] is non-zero. */
-                                                if(i == 15){
-                                                        cpu->TFlag = ret & 0x1;
-                                                        ret &= 0xFFFFFFFE;
-                                                        //DEBUG_LOG(ARM11, "In %s, TFlag ret=0x%x\n", __FUNCTION__, ret);
-                                                }
-
-                                                cpu->Reg[i] = ret;
-                                                addr += 4;
-                                                if ((addr & 0xfff) == 0) {
-                                                        fault = check_address_validity(cpu, addr, &phys_addr, 1);
-                                                } else {
-                                                        phys_addr += 4;
-                                                }
-                                        }
-                                }
-                        } else if (BIT(inst, 22) && BIT(inst, 15)) {
-                                for( i = 0; i < 15; i ++ ){
-                                        if(BIT(inst, i)){
-                                                #if 0
-                                                fault = check_address_validity(cpu, addr, &phys_addr, 1);
-                                                if (fault) {
-                                                        goto MMU_EXCEPTION;
-                                                }
-                                                #endif
-                                                fault = interpreter_read_memory(addr, phys_addr, ret, 32);
-                                                //if (fault) goto MMU_EXCEPTION;
-                                                cpu->Reg[i] = ret;
-                                                addr += 4;
-                                                if ((addr & 0xfff) == 0) {
-                                                        fault = check_address_validity(cpu, addr, &phys_addr, 1);
-                                                } else {
-                                                        phys_addr += 4;
-                                                }
-                                        }
-                                }
-                                
-                                if (CurrentModeHasSPSR) {
-                                        cpu->Cpsr = cpu->Spsr_copy;
-                                        switch_mode(cpu, cpu->Cpsr & 0x1f);
-                                        LOAD_NZCVT;
-                                }
-                                #if 0
-                                fault = check_address_validity(cpu, addr, &phys_addr, 1);
-                                if (fault) {
-                                        goto MMU_EXCEPTION;
-                                }
-                                #endif
-                                fault = interpreter_read_memory(addr, phys_addr, ret, 32);
-                                if (fault) {
-                                        goto MMU_EXCEPTION;
-                                }
-                                cpu->Reg[15] = ret;
-                                #if 0
-                                addr += 4;
-                                phys_addr += 4;
-                                #endif
-                        }
-                        if (BIT(inst, 15)) {
-                                INC_PC(sizeof(ldst_inst));
-                                goto DISPATCH;
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(ldst_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        SXTH_INST:
-        {
-                INC_ICOUNTER;
-                sxth_inst *inst_cream = (sxth_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        unsigned int operand2 = ROTATE_RIGHT_32(RM, 8 * inst_cream->rotate);
-                        if (BIT(operand2, 15)) {
-                                operand2 |= 0xffff0000;
-                        } else {
-                                operand2 &= 0xffff;
-                        }
-                        RD = operand2;
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(sxth_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        LDR_INST:
-        {
-                INC_ICOUNTER;
-                ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
-                //if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        fault = inst_cream->get_addr(cpu, inst_cream->inst, addr, phys_addr, 1);
-                        if (fault) goto MMU_EXCEPTION;
-                        unsigned int value;
-                        //bus_read(32, addr, &value);
-                        fault = interpreter_read_memory(addr, phys_addr, value, 32);
-                        if (BIT(CP15_REG(CP15_CONTROL), 22) == 1)
-                                cpu->Reg[BITS(inst_cream->inst, 12, 15)] = value;
-                        else {
-                                value = ROTATE_RIGHT_32(value,(8*(addr&0x3)));
-                                cpu->Reg[BITS(inst_cream->inst, 12, 15)] = value;
-                        }
-                        if (BITS(inst_cream->inst, 12, 15) == 15) {
-                                /* For armv5t, should enter thumb when bits[0] is non-zero. */
-                                cpu->TFlag = value & 0x1;
-                                cpu->Reg[15] &= 0xFFFFFFFE;
-                                INC_PC(sizeof(ldst_inst));
-                                goto DISPATCH;
-                        }
-                //}
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(ldst_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        LDRCOND_INST:
-        {
-                INC_ICOUNTER;
-                ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
-                if (CondPassed(cpu, inst_base->cond)) {
-                        fault = inst_cream->get_addr(cpu, inst_cream->inst, addr, phys_addr, 1);
-                        if (fault) goto MMU_EXCEPTION;
-                        unsigned int value;
-                        //bus_read(32, addr, &value);
-                        fault = interpreter_read_memory(addr, phys_addr, value, 32);
-                        if (BIT(CP15_REG(CP15_CONTROL), 22) == 1)
-                                cpu->Reg[BITS(inst_cream->inst, 12, 15)] = value;
-                        else {
-                                value = ROTATE_RIGHT_32(value,(8*(addr&0x3)));
-                                cpu->Reg[BITS(inst_cream->inst, 12, 15)] = value;
+                goto END;
+
+        inst_base = (arm_inst *)&inst_buf[ptr];
+        GOTO_NEXT_INST;
+    }
+    ADC_INST:
+    {
+        adc_inst *inst_cream = (adc_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            lop = RN;
+            unsigned int sht_op = SHIFTER_OPERAND;
+            rop = SHIFTER_OPERAND + cpu->CFlag;
+            RD = dst = lop + rop;
+            if (inst_cream->S && (inst_cream->Rd == 15)) {
+                if (CurrentModeHasSPSR) {
+                    cpu->Cpsr = cpu->Spsr_copy;
+                    switch_mode(cpu, cpu->Spsr_copy & 0x1f);
+                    LOAD_NZCVT;
+                }
+            } else if (inst_cream->S) {
+                UPDATE_NFLAG(dst);
+                UPDATE_ZFLAG(dst);
+                UPDATE_CFLAG_CARRY_FROM_ADD(lop, sht_op, cpu->CFlag);
+                UPDATE_VFLAG((int)dst, (int)lop, (int)rop);
+            }
+            if (inst_cream->Rd == 15) {
+                INC_PC(sizeof(adc_inst));
+                goto DISPATCH;
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(adc_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    ADD_INST:
+    {
+        add_inst *inst_cream = (add_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            lop = RN;
+            if (inst_cream->Rn == 15) {
+                lop += 2 * GET_INST_SIZE(cpu);
+            }
+            rop = SHIFTER_OPERAND;
+            RD = dst = lop + rop;
+            if (inst_cream->S && (inst_cream->Rd == 15)) {
+                if (CurrentModeHasSPSR) {
+                    cpu->Cpsr = cpu->Spsr_copy;
+                    switch_mode(cpu, cpu->Cpsr & 0x1f);
+                    LOAD_NZCVT;
+                }
+            } else if (inst_cream->S) {
+                UPDATE_NFLAG(dst);
+                UPDATE_ZFLAG(dst);
+                UPDATE_CFLAG(dst, lop, rop);
+                UPDATE_VFLAG((int)dst, (int)lop, (int)rop);
+            }
+            if (inst_cream->Rd == 15) {
+                INC_PC(sizeof(add_inst));
+                goto DISPATCH;
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(add_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    AND_INST:
+    {
+        and_inst *inst_cream = (and_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            lop = RN;
+            rop = SHIFTER_OPERAND;
+            RD = dst = lop & rop;
+            if (inst_cream->S && (inst_cream->Rd == 15)) {
+                if (CurrentModeHasSPSR) {
+                    cpu->Cpsr = cpu->Spsr_copy;
+                    switch_mode(cpu, cpu->Cpsr & 0x1f);
+                    LOAD_NZCVT;
+                }
+            } else if (inst_cream->S) {
+                UPDATE_NFLAG(dst);
+                UPDATE_ZFLAG(dst);
+                UPDATE_CFLAG_WITH_SC;
+            }
+            if (inst_cream->Rd == 15) {
+                INC_PC(sizeof(and_inst));
+                goto DISPATCH;
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(and_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    BBL_INST:
+    {
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            bbl_inst *inst_cream = (bbl_inst *)inst_base->component;
+            if (inst_cream->L) {
+                LINK_RTN_ADDR;
+            }
+            SET_PC;
+            INC_PC(sizeof(bbl_inst));
+            goto DISPATCH;
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(bbl_inst));
+        goto DISPATCH;
+    }
+    BIC_INST:
+    {
+        bic_inst *inst_cream = (bic_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            lop = RN;
+            if (inst_cream->Rn == 15) {
+                lop += 2 * GET_INST_SIZE(cpu);
+            }
+            rop = SHIFTER_OPERAND;
+            RD = dst = lop & (~rop);
+            if ((inst_cream->S) && (inst_cream->Rd == 15)) {
+                if (CurrentModeHasSPSR) {
+                    cpu->Cpsr = cpu->Spsr_copy;
+                    switch_mode(cpu, cpu->Spsr_copy & 0x1f);
+                    LOAD_NZCVT;
+                }
+            } else if (inst_cream->S) {
+                UPDATE_NFLAG(dst);
+                UPDATE_ZFLAG(dst);
+                UPDATE_CFLAG_WITH_SC;
+            }
+            if (inst_cream->Rd == 15) {
+                INC_PC(sizeof(bic_inst));
+                goto DISPATCH;
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(bic_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    BKPT_INST:
+    BLX_INST:
+    {
+        blx_inst *inst_cream = (blx_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            unsigned int inst = inst_cream->inst;
+            if (BITS(inst, 20, 27) == 0x12 && BITS(inst, 4, 7) == 0x3) {
+                cpu->Reg[14] = (cpu->Reg[15] + GET_INST_SIZE(cpu));
+                if(cpu->TFlag)
+                    cpu->Reg[14] |= 0x1;
+                cpu->Reg[15] = cpu->Reg[inst_cream->val.Rm] & 0xfffffffe;
+                cpu->TFlag = cpu->Reg[inst_cream->val.Rm] & 0x1;
+            } else {
+                cpu->Reg[14] = (cpu->Reg[15] + GET_INST_SIZE(cpu));
+                cpu->TFlag = 0x1;
+                int signed_int = inst_cream->val.signed_immed_24;
+                signed_int = (signed_int) & 0x800000 ? (0x3F000000 | signed_int) : signed_int;
+                signed_int = signed_int << 2;
+                cpu->Reg[15] = cpu->Reg[15] + 8 + signed_int + (BIT(inst, 24) << 1);
+            }
+            INC_PC(sizeof(blx_inst));
+            goto DISPATCH;
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(blx_inst));
+        goto DISPATCH;
+    }
+    BX_INST:
+    {
+        bx_inst *inst_cream = (bx_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            if (inst_cream->Rm == 15)
+                LOG_WARNING(Core_ARM11, "BX at pc %x: use of Rm = R15 is discouraged", cpu->Reg[15]);
+            cpu->TFlag = cpu->Reg[inst_cream->Rm] & 0x1;
+            cpu->Reg[15] = cpu->Reg[inst_cream->Rm] & 0xfffffffe;
+            INC_PC(sizeof(bx_inst));
+            goto DISPATCH;
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(bx_inst));
+        goto DISPATCH;
+    }
+    BXJ_INST:
+    CDP_INST:
+    {
+        cdp_inst *inst_cream = (cdp_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            // Undefined instruction here
+            cpu->NumInstrsToExecute = 0;
+            return num_instrs;
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(cdp_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+
+    CLREX_INST:
+    {
+        remove_exclusive(cpu, 0);
+        cpu->exclusive_state = 0;
+
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(clrex_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    CLZ_INST:
+    {
+        clz_inst *inst_cream = (clz_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            RD = clz(RM);
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(clz_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    CMN_INST:
+    {
+        cmn_inst *inst_cream = (cmn_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            lop = RN;
+            rop = SHIFTER_OPERAND;
+            dst = lop + rop;
+            UPDATE_NFLAG(dst);
+            UPDATE_ZFLAG(dst);
+            UPDATE_CFLAG(dst, lop, rop);
+            UPDATE_VFLAG((int)dst, (int)lop, (int)rop);
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(cmn_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    CMP_INST:
+    {
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            cmp_inst *inst_cream = (cmp_inst *)inst_base->component;
+            lop = RN;
+            if (inst_cream->Rn == 15) {
+                lop += 2 * GET_INST_SIZE(cpu);
+            }
+            rop = SHIFTER_OPERAND;
+            dst = lop - rop;
+
+            UPDATE_NFLAG(dst);
+            UPDATE_ZFLAG(dst);
+            UPDATE_CFLAG_NOT_BORROW_FROM(lop, rop);
+            UPDATE_VFLAG_OVERFLOW_FROM(dst, lop, rop);
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(cmp_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    CPS_INST:
+    {
+        cps_inst *inst_cream = (cps_inst *)inst_base->component;
+        uint32_t aif_val = 0;
+        uint32_t aif_mask = 0;
+        if (InAPrivilegedMode(cpu)) {
+            if (inst_cream->imod1) {
+                if (inst_cream->A) {
+                    aif_val |= (inst_cream->imod0 << 8);
+                    aif_mask |= 1 << 8;
+                }
+                if (inst_cream->I) {
+                    aif_val |= (inst_cream->imod0 << 7);
+                    aif_mask |= 1 << 7;
+                }
+                if (inst_cream->F) {
+                    aif_val |= (inst_cream->imod0 << 6);
+                    aif_mask |= 1 << 6;
+                }
+                aif_mask = ~aif_mask;
+                cpu->Cpsr = (cpu->Cpsr & aif_mask) | aif_val;
+            }
+            if (inst_cream->mmod) {
+                cpu->Cpsr = (cpu->Cpsr & 0xffffffe0) | inst_cream->mode;
+                switch_mode(cpu, inst_cream->mode);
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(cps_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    CPY_INST:
+    {
+        mov_inst *inst_cream = (mov_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            RD = SHIFTER_OPERAND;
+            if ((inst_cream->Rd == 15)) {
+                INC_PC(sizeof(mov_inst));
+                goto DISPATCH;
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(mov_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    EOR_INST:
+    {
+        eor_inst *inst_cream = (eor_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            lop = RN;
+            if (inst_cream->Rn == 15) {
+                lop += 2 * GET_INST_SIZE(cpu);
+            }
+            rop = SHIFTER_OPERAND;
+            RD = dst = lop ^ rop;
+            if (inst_cream->S && (inst_cream->Rd == 15)) {
+                if (CurrentModeHasSPSR) {
+                    cpu->Cpsr = cpu->Spsr_copy;
+                    switch_mode(cpu, cpu->Spsr_copy & 0x1f);
+                    LOAD_NZCVT;
+                }
+            } else if (inst_cream->S) {
+                UPDATE_NFLAG(dst);
+                UPDATE_ZFLAG(dst);
+                UPDATE_CFLAG_WITH_SC;
+            }
+            if (inst_cream->Rd == 15) {
+                INC_PC(sizeof(eor_inst));
+                goto DISPATCH;
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(eor_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    LDC_INST:
+    {
+        // Instruction not implemented
+        //LOG_CRITICAL(Core_ARM11, "unimplemented instruction");
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(ldc_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    LDM_INST:
+    {
+        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            int i;
+            unsigned int ret;
+            fault = inst_cream->get_addr(cpu, inst_cream->inst, addr, phys_addr, 1);
+            if (fault) {
+                goto MMU_EXCEPTION;
+            }
+            unsigned int inst = inst_cream->inst;
+            if (BIT(inst, 22) && !BIT(inst, 15)) {
+                for (i = 0; i < 13; i++) {
+                    if(BIT(inst, i)){
+                        fault = interpreter_read_memory(addr, phys_addr, ret, 32);
+                        cpu->Reg[i] = ret;
+                        addr += 4;
+                        if ((addr & 0xfff) == 0) {
+                            fault = check_address_validity(cpu, addr, &phys_addr, 1);
+                        } else {
+                            phys_addr += 4;
+                        }
+                    }
+                }
+                if (BIT(inst, 13)) {
+                    fault = interpreter_read_memory(addr, phys_addr, ret, 32);
+
+                    if (cpu->Mode == USER32MODE) 
+                        cpu->Reg[13] = ret;
+                    else
+                        cpu->Reg_usr[0] = ret;
+                    addr += 4;
+                    if ((addr & 0xfff) == 0) {
+                        fault = check_address_validity(cpu, addr, &phys_addr, 1);
+                    } else {
+                        phys_addr += 4;
+                    }
+                }
+                if (BIT(inst, 14)) {
+                    fault = interpreter_read_memory(addr, phys_addr, ret, 32);
+
+                    if (cpu->Mode == USER32MODE) 
+                        cpu->Reg[14] = ret;
+                    else
+                        cpu->Reg_usr[1] = ret;
+                }
+            } else if (!BIT(inst, 22)) {
+                for( i = 0; i < 16; i ++ ){
+                    if(BIT(inst, i)){
+                        fault = interpreter_read_memory(addr, phys_addr, ret, 32);
+                        if (fault) goto MMU_EXCEPTION;
+
+                        // For armv5t, should enter thumb when bits[0] is non-zero.
+                        if(i == 15){
+                            cpu->TFlag = ret & 0x1;
+                            ret &= 0xFFFFFFFE;
                         }
 
-                        if (BITS(inst_cream->inst, 12, 15) == 15) {
-                                /* For armv5t, should enter thumb when bits[0] is non-zero. */
-                                cpu->TFlag = value & 0x1;
-                                cpu->Reg[15] &= 0xFFFFFFFE;
-                                INC_PC(sizeof(ldst_inst));
-                                goto DISPATCH;
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(ldst_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        UXTH_INST:
-        {
-                INC_ICOUNTER;
-                uxth_inst *inst_cream = (uxth_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        unsigned int operand2 = ROTATE_RIGHT_32(RM, 8 * inst_cream->rotate) 
-                                                & 0xffff;
-                        RD = operand2;
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(uxth_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        UXTAH_INST:
-        {
-                INC_ICOUNTER;
-                uxtah_inst *inst_cream = (uxtah_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        unsigned int operand2 = ROTATE_RIGHT_32(RM, 8 * inst_cream->rotate) 
-                                                & 0xffff;
-                        RD = RN + operand2;
-                        if (inst_cream->Rn == 15 || inst_cream->Rm == 15) {
-                                LOG_ERROR(Core_ARM11, "invalid operands for UXTAH");
-                                CITRA_IGNORE_EXIT(-1);
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(uxtah_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        LDRB_INST:
-        {
-                INC_ICOUNTER;
-                ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        fault = inst_cream->get_addr(cpu, inst_cream->inst, addr, phys_addr, 1);
-                        if (fault) goto MMU_EXCEPTION;
-                        unsigned int value;
-                        fault = interpreter_read_memory(addr, phys_addr, value, 8);
-                        if (fault) goto MMU_EXCEPTION;
-                        //bus_read(8, addr, &value);
-                        cpu->Reg[BITS(inst_cream->inst, 12, 15)] = value;
-                        if (BITS(inst_cream->inst, 12, 15) == 15) {
-                                INC_PC(sizeof(ldst_inst));
-                                goto DISPATCH;
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(ldst_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        LDRBT_INST:
-        {
-                INC_ICOUNTER;
-                ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        fault = inst_cream->get_addr(cpu, inst_cream->inst, addr, phys_addr, 1);
-                        if (fault) goto MMU_EXCEPTION;
-                        unsigned int value;
-                        fault = interpreter_read_memory(addr, phys_addr, value, 8);
-                        if (fault) goto MMU_EXCEPTION;
-                        cpu->Reg[BITS(inst_cream->inst, 12, 15)] = value;
-                        if (BITS(inst_cream->inst, 12, 15) == 15) {
-                                INC_PC(sizeof(ldst_inst));
-                                goto DISPATCH;
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(ldst_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        LDRD_INST:
-        {
-                INC_ICOUNTER;
-                ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        /* Should check if RD is even-numbered, Rd != 14, addr[0:1] == 0, (CP15_reg1_U == 1 || addr[2] == 0) */
-                        fault = inst_cream->get_addr(cpu, inst_cream->inst, addr, phys_addr, 1);
-                        if (fault) goto MMU_EXCEPTION;
-                        uint32_t rear_phys_addr;
-                         fault = check_address_validity(cpu, addr + 4, &rear_phys_addr, 1);
-                        if(fault){
-                                LOG_ERROR(Core_ARM11, "mmu fault , should rollback the above get_addr\n");
-                                CITRA_IGNORE_EXIT(-1);
-                                goto MMU_EXCEPTION;
-                        }
-                        unsigned int value;
-                        fault = interpreter_read_memory(addr, phys_addr, value, 32);
-                        if (fault) goto MMU_EXCEPTION;
-                        cpu->Reg[BITS(inst_cream->inst, 12, 15)] = value;
-                        fault = interpreter_read_memory(addr + 4, rear_phys_addr, value, 32);
-                        if (fault) goto MMU_EXCEPTION;
-                        cpu->Reg[BITS(inst_cream->inst, 12, 15) + 1] = value;
-                        /* No dispatch since this operation should not modify R15 */
-                }
-                cpu->Reg[15] += 4;
-                INC_PC(sizeof(ldst_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-
-        LDREX_INST:
-        {
-                INC_ICOUNTER;
-                ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        addr = cpu->Reg[BITS(inst_cream->inst, 16, 19)];
-                        fault = check_address_validity(cpu, addr, &phys_addr, 1);
-                        if (fault) goto MMU_EXCEPTION;
-                        unsigned int value;
-                        fault = interpreter_read_memory(addr, phys_addr, value, 32);
-                        if (fault) goto MMU_EXCEPTION;
-
-                        add_exclusive_addr(cpu, phys_addr);
-                        cpu->exclusive_state = 1;
-
-                        //bus_read(32, addr, &value);
-                        cpu->Reg[BITS(inst_cream->inst, 12, 15)] = value;
-                        if (BITS(inst_cream->inst, 12, 15) == 15) {
-                                INC_PC(sizeof(ldst_inst));
-                                goto DISPATCH;
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(ldst_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        LDREXB_INST:
-        {
-                INC_ICOUNTER;
-                ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        addr = cpu->Reg[BITS(inst_cream->inst, 16, 19)];
-                        fault = check_address_validity(cpu, addr, &phys_addr, 1);
-                        if (fault) goto MMU_EXCEPTION;
-                        unsigned int value;
-                        fault = interpreter_read_memory(addr, phys_addr, value, 8);
-                        if (fault) goto MMU_EXCEPTION;
-                        
-                        add_exclusive_addr(cpu, phys_addr);
-                        cpu->exclusive_state = 1;
-
-                        //bus_read(8, addr, &value);
-                        cpu->Reg[BITS(inst_cream->inst, 12, 15)] = value;
-                        if (BITS(inst_cream->inst, 12, 15) == 15) {
-                                INC_PC(sizeof(ldst_inst));
-                                goto DISPATCH;
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(ldst_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        LDRH_INST:
-        {
-                INC_ICOUNTER;
-                ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        fault = inst_cream->get_addr(cpu, inst_cream->inst, addr, phys_addr, 1);
-                        if (fault) goto MMU_EXCEPTION;
-                        unsigned int value = 0;
-                        fault = interpreter_read_memory(addr, phys_addr, value, 16);
-//                      fault = interpreter_read_memory(addr, value, 32);
-                        if (fault) goto MMU_EXCEPTION;
-                        //if (value == 0xffff && cpu->icounter > 190000000 && cpu->icounter < 210000000) {
-                        //      value = 0xffffffff;
-                        //}
-                        //bus_read(16, addr, &value);
-//                      cpu->Reg[BITS(inst_cream->inst, 12, 15)] = value & 0xffff;
-                        cpu->Reg[BITS(inst_cream->inst, 12, 15)] = value;
-                        if (BITS(inst_cream->inst, 12, 15) == 15) {
-                                INC_PC(sizeof(ldst_inst));
-                                goto DISPATCH;
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(ldst_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        LDRSB_INST:
-        {
-                INC_ICOUNTER;
-                ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        fault = inst_cream->get_addr(cpu, inst_cream->inst, addr, phys_addr, 1);
-                        if (fault) goto MMU_EXCEPTION;
-                        unsigned int value;
-//                      DEBUG_LOG(ARM11, "ldrsb addr is %x\n", addr);
-                        fault = interpreter_read_memory(addr, phys_addr, value, 8);
-                        if (fault) goto MMU_EXCEPTION;
-                        //bus_read(8, addr, &value);
-                        if (BIT(value, 7)) {
-                                value |= 0xffffff00;
-                        }
-                        cpu->Reg[BITS(inst_cream->inst, 12, 15)] = value;
-                        if (BITS(inst_cream->inst, 12, 15) == 15) {
-                                INC_PC(sizeof(ldst_inst));
-                                goto DISPATCH;
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(ldst_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        LDRSH_INST:
-        {
-                INC_ICOUNTER;
-                ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        fault = inst_cream->get_addr(cpu, inst_cream->inst, addr, phys_addr, 1);
-                        if (fault) goto MMU_EXCEPTION;
-                        unsigned int value;
-                        fault = interpreter_read_memory(addr, phys_addr, value, 16);
-                        if (fault) goto MMU_EXCEPTION;
-                        //bus_read(16, addr, &value);
-                        if (BIT(value, 15)) {
-                                value |= 0xffff0000;
-                        }
-                        cpu->Reg[BITS(inst_cream->inst, 12, 15)] = value;
-                        if (BITS(inst_cream->inst, 12, 15) == 15) {
-                                INC_PC(sizeof(ldst_inst));
-                                goto DISPATCH;
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(ldst_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        LDRT_INST:
-        {
-                INC_ICOUNTER;
-                ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        fault = inst_cream->get_addr(cpu, inst_cream->inst, addr, phys_addr, 1);
-                        if (fault) goto MMU_EXCEPTION;
-                        unsigned int value;
-                        fault = interpreter_read_memory(addr, phys_addr, value, 32);
-                        if (fault) goto MMU_EXCEPTION;
-                        cpu->Reg[BITS(inst_cream->inst, 12, 15)] = value;
-
-                        if (BIT(CP15_REG(CP15_CONTROL), 22) == 1)
-                                cpu->Reg[BITS(inst_cream->inst, 12, 15)] = value;
-                        else
-                                cpu->Reg[BITS(inst_cream->inst, 12, 15)] = ROTATE_RIGHT_32(value,(8*(addr&0x3))) ;
-
-                        if (BITS(inst_cream->inst, 12, 15) == 15) {
-                                INC_PC(sizeof(ldst_inst));
-                                goto DISPATCH;
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(ldst_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        MCR_INST:
-        {
-                INC_ICOUNTER;
-                /* NOT IMPL */
-                mcr_inst *inst_cream = (mcr_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        unsigned int inst = inst_cream->inst;
-                        if (inst_cream->Rd == 15) {
-                                DEBUG_MSG;
-                        } else {
-                                if (inst_cream->cp_num == 15) {
-                                        if(CRn == 0 && OPCODE_2 == 0 && CRm == 0) {
-                                                //LET(RD, CONST(0x0007b000));
-                                                //LET(RD, CONST(0x410FB760));
-                                                //LET(CP15_MAIN_ID, R(RD));
-                                                CP15_REG(CP15_MAIN_ID) = RD;
-                                        } else if (CRn == 1 && CRm == 0 && OPCODE_2 == 1) {
-                                                //LET(RD, R(CP15_CONTROL));
-                                                CP15_REG(CP15_AUXILIARY_CONTROL) = RD;
-                                        } else if (CRn == 1 && CRm == 0 && OPCODE_2 == 2) {
-                                                //LET(RD, R(CP15_CONTROL));
-                                                CP15_REG(CP15_COPROCESSOR_ACCESS_CONTROL) = RD;
-                                        } else if(CRn == 1 && CRm == 0 && OPCODE_2 == 0) {
-                                                //LET(CP15_CONTROL, R(RD));
-                                                CP15_REG(CP15_CONTROL) = RD;
-                                        } else if (CRn == 3 && CRm == 0 && OPCODE_2 == 0) {
-                                                //LET(CP15_DOMAIN_ACCESS_CONTROL, R(RD));
-                                                CP15_REG(CP15_DOMAIN_ACCESS_CONTROL) = RD;
-                                        } else if (CRn == 2 && CRm == 0 && OPCODE_2 == 0) {
-                                                //LET(CP15_TRANSLATION_BASE_TABLE_0, R(RD));
-                                                CP15_REG(CP15_TRANSLATION_BASE_TABLE_0) = RD;
-                                        } else if (CRn == 2 && CRm == 0 && OPCODE_2 == 1) {
-                                                //LET(CP15_TRANSLATION_BASE_TABLE_1, R(RD));
-                                                CP15_REG(CP15_TRANSLATION_BASE_TABLE_1) = RD;
-                                        } else if (CRn == 2 && CRm == 0 && OPCODE_2 == 2) {
-                                                //LET(CP15_TRANSLATION_BASE_CONTROL, R(RD));
-                                                CP15_REG(CP15_TRANSLATION_BASE_CONTROL) = RD;
-                                        } else if(CRn == MMU_CACHE_OPS){
-                                                //SKYEYE_WARNING("cache operation have not implemented.\n");
-                                        } else if(CRn == MMU_TLB_OPS){
-                                                switch (CRm) {
-                                                case 5: /* ITLB */
-                                                        switch(OPCODE_2){
-                                                        case 0: /* invalidate all */
-                                                                //invalidate_all_tlb(state);
-                                                                LOG_DEBUG(Core_ARM11, "{TLB} [INSN] invalidate all");
-                                                                //remove_tlb(INSN_TLB);
-                                                                //erase_all(core, INSN_TLB);
-                                                                break;
-                                                        case 1: /* invalidate by MVA */
-                                                                //invalidate_by_mva(state, value);
-                                                                //DEBUG_LOG(ARM11, "{TLB} [INSN] invalidate by mva\n");
-                                                                //remove_tlb_by_mva(RD, INSN_TLB);
-                                                                //erase_by_mva(core, RD, INSN_TLB);
-                                                                break;
-                                                        case 2: /* invalidate by asid */
-                                                                //invalidate_by_asid(state, value);
-                                                                //DEBUG_LOG(ARM11, "{TLB} [INSN] invalidate by asid\n");
-                                                                //erase_by_asid(core, RD, INSN_TLB);
-                                                                break;
-                                                        default:
-                                                                break;
-                                                        }
-
-                                                        break;
-                                                case 6: /* DTLB */
-                                                        switch(OPCODE_2){
-                                                        case 0: /* invalidate all */
-                                                                //invalidate_all_tlb(state);
-                                                                //remove_tlb(DATA_TLB);
-                                                                //erase_all(core, DATA_TLB);
-                                                                LOG_DEBUG(Core_ARM11, "{TLB} [DATA] invalidate all");
-                                                                break;
-                                                        case 1: /* invalidate by MVA */
-                                                                //invalidate_by_mva(state, value);
-                                                                //remove_tlb_by_mva(RD, DATA_TLB);
-                                                                //erase_by_mva(core, RD, DATA_TLB);
-                                                                //DEBUG_LOG(ARM11, "{TLB} [DATA] invalidate by mva\n");
-                                                                break;
-                                                        case 2: /* invalidate by asid */
-                                                                //invalidate_by_asid(state, value);
-                                                                //remove_tlb_by_asid(RD, DATA_TLB);
-                                                                //erase_by_asid(core, RD, DATA_TLB);
-                                                                //DEBUG_LOG(ARM11, "{TLB} [DATA] invalidate by asid\n");
-                                                                break;
-                                                        default:
-                                                                break;
-                                                        }
-                                                        break;
-                                                case 7: /* UNIFILED TLB */
-                                                        switch(OPCODE_2){
-                                                        case 0: /* invalidate all */
-                                                                //invalidate_all_tlb(state);
-                                                                //erase_all(core, INSN_TLB);
-                                                                //erase_all(core, DATA_TLB);
-                                                                //remove_tlb(DATA_TLB);
-                                                                //remove_tlb(INSN_TLB);
-                                                                //DEBUG_LOG(ARM11, "{TLB} [UNIFILED] invalidate all\n");
-                                                                break;
-                                                        case 1: /* invalidate by MVA */
-                                                                //invalidate_by_mva(state, value);
-                                                                //erase_by_mva(core, RD, DATA_TLB);
-                                                                //erase_by_mva(core, RD, INSN_TLB);
-                                                                LOG_DEBUG(Core_ARM11, "{TLB} [UNIFILED] invalidate by mva");
-                                                                break;
-                                                        case 2: /* invalidate by asid */
-                                                                //invalidate_by_asid(state, value);
-                                                                //erase_by_asid(core, RD, DATA_TLB);
-                                                                //erase_by_asid(core, RD, INSN_TLB);
-                                                                LOG_DEBUG(Core_ARM11, "{TLB} [UNIFILED] invalidate by asid");
-                                                                break;
-                                                        default:
-                                                                break;
-                                                        }
-                                                        break;
-                                                default:
-                                                        break;
-                                                }
-                                        } else if(CRn == MMU_PID){
-                                                if(OPCODE_2 == 0)
-                                                        CP15_REG(CP15_PID) = RD;
-                                                else if(OPCODE_2 == 1)
-                                                        CP15_REG(CP15_CONTEXT_ID) = RD;
-                                                else if(OPCODE_2 == 3){
-                                                        CP15_REG(CP15_THREAD_URO) = RD;
-                                                }
-                                                else{
-                                                        printf ("mmu_mcr wrote UNKNOWN - reg %d\n", CRn);
-                                                }
-
-                                        } else {
-                                                LOG_ERROR(Core_ARM11, "mcr CRn=%d, CRm=%d OP2=%d is not implemented", CRn, CRm, OPCODE_2);
-                                        }
-                                }
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(mcr_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        MCRR_INST:
-        MLA_INST:
-        {
-                INC_ICOUNTER;
-                mla_inst *inst_cream = (mla_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        uint64_t rm = RM;
-                        uint64_t rs = RS;
-                        uint64_t rn = RN;
-                        if (inst_cream->Rm == 15 || inst_cream->Rs == 15 || inst_cream->Rn == 15) {
-                                LOG_ERROR(Core_ARM11, "invalid operands for MLA");
-                                CITRA_IGNORE_EXIT(-1);
-                        }
-//                      RD = dst = RM * RS + RN;
-                        RD = dst = static_cast<uint32_t>((rm * rs + rn) & 0xffffffff);
-                        if (inst_cream->S) {
-                                UPDATE_NFLAG(dst);
-                                UPDATE_ZFLAG(dst);
-                        }
-                        if (inst_cream->Rd == 15) {
-                                INC_PC(sizeof(mla_inst));
-                                goto DISPATCH;
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(mla_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        MOV_INST:
-        {
-//              DEBUG_LOG(ARM11, "mov inst\n");
-//              DEBUG_LOG(ARM11, "pc:       %x\n", cpu->Reg[15]);
-//              debug_function(cpu);
-//              cpu->icount ++;
-                INC_ICOUNTER;
-                mov_inst *inst_cream = (mov_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        RD = dst = SHIFTER_OPERAND;
-                        if (inst_cream->S && (inst_cream->Rd == 15)) {
-                                /* cpsr = spsr */
-                                if (CurrentModeHasSPSR) {
-                                        cpu->Cpsr = cpu->Spsr_copy;
-                                        switch_mode(cpu, cpu->Spsr_copy & 0x1f);
-                                        LOAD_NZCVT;
-                                }
-                        } else if (inst_cream->S) {
-                                UPDATE_NFLAG(dst);
-                                UPDATE_ZFLAG(dst);
-                                UPDATE_CFLAG_WITH_SC;
-                        }
-                        if (inst_cream->Rd == 15) {
-                                INC_PC(sizeof(mov_inst));
-                                goto DISPATCH;
-                        }
-//                              return;
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(mov_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        MRC_INST:
-        {
-                INC_ICOUNTER;
-                /* NOT IMPL */
-                mrc_inst *inst_cream = (mrc_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        unsigned int inst = inst_cream->inst;
-                        if (inst_cream->Rd == 15) {
-                                DEBUG_MSG;
-                        }
-                        if (inst_cream->inst == 0xeef04a10) {
-                                /* undefined instruction fmrx */
-                                RD = 0x20000000;
-                                CITRA_IGNORE_EXIT(-1);
-                                goto END;
-                        } else {
-                                if (inst_cream->cp_num == 15) {
-                                        if(CRn == 0 && OPCODE_2 == 0 && CRm == 0) {
-                                                //LET(RD, CONST(0x0007b000));
-                                                //LET(RD, CONST(0x410FB760));
-                                                //LET(RD, R(CP15_MAIN_ID));
-                                                RD = cpu->CP15[CP15(CP15_MAIN_ID)];
-                                        } else if (CRn == 1 && CRm == 0 && OPCODE_2 == 0) {
-                                                //LET(RD, R(CP15_CONTROL));
-                                                RD = cpu->CP15[CP15(CP15_CONTROL)];
-                                        } else if (CRn == 1 && CRm == 0 && OPCODE_2 == 1) {
-                                                //LET(RD, R(CP15_CONTROL));
-                                                RD = cpu->CP15[CP15(CP15_AUXILIARY_CONTROL)];
-                                        } else if (CRn == 1 && CRm == 0 && OPCODE_2 == 2) {
-                                                //LET(RD, R(CP15_CONTROL));
-                                                RD = cpu->CP15[CP15(CP15_COPROCESSOR_ACCESS_CONTROL)];
-                                        } else if (CRn == 3 && CRm == 0 && OPCODE_2 == 0) {
-                                                //LET(RD, R(CP15_DOMAIN_ACCESS_CONTROL));
-                                                RD = cpu->CP15[CP15(CP15_DOMAIN_ACCESS_CONTROL)];
-                                        } else if (CRn == 2 && CRm == 0 && OPCODE_2 == 0) {
-                                                //LET(RD, R(CP15_TRANSLATION_BASE_TABLE_0));
-                                                RD = cpu->CP15[CP15(CP15_TRANSLATION_BASE_TABLE_0)];
-                                        } else if (CRn == 5 && CRm == 0 && OPCODE_2 == 0) {
-                                                //LET(RD, R(CP15_FAULT_STATUS));
-                                                RD = cpu->CP15[CP15(CP15_FAULT_STATUS)];
-                                        } else if (CRn == 6 && CRm == 0 && OPCODE_2 == 0) {
-                                                //LET(RD, R(CP15_FAULT_ADDRESS));
-                                                RD = cpu->CP15[CP15(CP15_FAULT_ADDRESS)];
-                                        } else if (CRn == 0 && CRm == 0 && OPCODE_2 == 1) {
-                                                //LET(RD, R(CP15_CACHE_TYPE));
-                                                RD = cpu->CP15[CP15(CP15_CACHE_TYPE)];
-                                        } else if (CRn == 5 && CRm == 0 && OPCODE_2 == 1) {
-                                                //LET(RD, R(CP15_INSTR_FAULT_STATUS));
-                                                RD = cpu->CP15[CP15(CP15_INSTR_FAULT_STATUS)];
-                                        } else if (CRn == 13) {
-                                                if(OPCODE_2 == 0)
-                                                        RD = CP15_REG(CP15_PID);
-                                                else if(OPCODE_2 == 1)
-                                                        RD = CP15_REG(CP15_CONTEXT_ID);
-                                                else if(OPCODE_2 == 3){
-                                                        RD = Memory::KERNEL_MEMORY_VADDR;
-                                                }
-                                                else{
-                                                        printf ("mmu_mrr wrote UNKNOWN - reg %d\n", CRn);
-                                                }
-                                        }
-                                        else {
-                                            LOG_ERROR(Core_ARM11, "mrc CRn=%d, CRm=%d, OP2=%d is not implemented", CRn, CRm, OPCODE_2);
-                                        }
-                                }
-                                //DEBUG_LOG(ARM11, "mrc is not implementated. CRn is %d, CRm is %d, OPCODE_2 is %d\n", CRn, CRm, OPCODE_2);
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(mrc_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        MRRC_INST:
-        MRS_INST:
-        {
-                INC_ICOUNTER;
-                mrs_inst *inst_cream = (mrs_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        if (inst_cream->R) {
-                                RD = cpu->Spsr_copy;
-                        } else {
-                                SAVE_NZCVT;
-                                RD = cpu->Cpsr;
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(mrs_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        MSR_INST:
-        {
-                INC_ICOUNTER;
-                msr_inst *inst_cream = (msr_inst *)inst_base->component;
-                const uint32_t UnallocMask = 0x06f0fc00, UserMask = 0xf80f0200, PrivMask = 0x000001df, StateMask = 0x01000020;
-                unsigned int inst = inst_cream->inst;
-                unsigned int operand;
-
-                if (BIT(inst, 25)) {
-                        int rot_imm = BITS(inst, 8, 11) * 2;
-                        //operand = ROTL(CONST(BITS(0, 7)), CONST(32 - rot_imm));
-                        operand = ROTATE_RIGHT_32(BITS(inst, 0, 7), rot_imm);
-                } else {
-                        //operand = R(RM);
-                        operand = cpu->Reg[BITS(inst, 0, 3)];
-                }
-                uint32_t byte_mask = (BIT(inst, 16) ? 0xff : 0) | (BIT(inst, 17) ? 0xff00 : 0)
-                                        | (BIT(inst, 18) ? 0xff0000 : 0) | (BIT(inst, 19) ? 0xff000000 : 0);
-                uint32_t mask;
-                if (!inst_cream->R) {
-                        if (InAPrivilegedMode(cpu)) {
-                                if ((operand & StateMask) != 0) {
-                                        /* UNPREDICTABLE */
-                                        DEBUG_MSG;
-                                } else
-                                        mask = byte_mask & (UserMask | PrivMask);
-                        } else {
-                                mask = byte_mask & UserMask;
-                        }
-                        //LET(CPSR_REG, OR(AND(R(CPSR_REG), COM(CONST(mask))), AND(operand, CONST(mask))));
-                        SAVE_NZCVT;
-
-                        cpu->Cpsr = (cpu->Cpsr & ~mask) | (operand & mask);
-                        switch_mode(cpu, cpu->Cpsr & 0x1f);
-                        LOAD_NZCVT;
-                } else {
-                        if (CurrentModeHasSPSR) {
-                                mask = byte_mask & (UserMask | PrivMask | StateMask);
-                                //LET(SPSR_REG, OR(AND(R(SPSR_REG), COM(CONST(mask))), AND(operand, CONST(mask))));
-                                cpu->Spsr_copy = (cpu->Spsr_copy & ~mask) | (operand & mask);
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(msr_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        MUL_INST:
-        {
-                INC_ICOUNTER;
-                mul_inst *inst_cream = (mul_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-//                      RD = dst = SHIFTER_OPERAND;
-                        uint64_t rm = RM;
-                        uint64_t rs = RS;
-                        RD = dst = static_cast<uint32_t>((rm * rs) & 0xffffffff);
-                        if (inst_cream->S) {
-                                UPDATE_NFLAG(dst);
-                                UPDATE_ZFLAG(dst);
-                        }
-                        if (inst_cream->Rd == 15) {
-                                INC_PC(sizeof(mul_inst));
-                                goto DISPATCH;
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(mul_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        MVN_INST:
-        {
-                INC_ICOUNTER;
-                mvn_inst *inst_cream = (mvn_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-//                      RD = dst = (SHIFTER_OPERAND ^ 0xffffffff);
-                        RD = dst = ~SHIFTER_OPERAND;
-                        if (inst_cream->S && (inst_cream->Rd == 15)) {
-                                /* cpsr = spsr */
-                                if (CurrentModeHasSPSR) {
-                                        cpu->Cpsr = cpu->Spsr_copy;
-                                        switch_mode(cpu, cpu->Spsr_copy & 0x1f);
-                                        LOAD_NZCVT;
-                                }
-                        } else if (inst_cream->S) {
-                                UPDATE_NFLAG(dst);
-                                UPDATE_ZFLAG(dst);
-                                UPDATE_CFLAG_WITH_SC;
-                        }
-                        if (inst_cream->Rd == 15) {
-                                INC_PC(sizeof(mvn_inst));
-                                goto DISPATCH;
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(mvn_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        ORR_INST:
-        {
-                INC_ICOUNTER;
-                orr_inst *inst_cream = (orr_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        lop = RN;
-                        rop = SHIFTER_OPERAND;
-//                      DEBUG_LOG(ARM11, "lop is %x, rop is %x, r2 is %x, r3 is %x\n", lop, rop, cpu->Reg[2], cpu->Reg[3]);
-                        RD = dst = lop | rop;
-                        if (inst_cream->S && (inst_cream->Rd == 15)) {
-                                /* cpsr = spsr*/
-                                if (CurrentModeHasSPSR) {
-                                        cpu->Cpsr = cpu->Spsr_copy;
-                                        switch_mode(cpu, cpu->Spsr_copy & 0x1f);
-                                        LOAD_NZCVT;
-                                }
-                        } else if (inst_cream->S) {
-                                UPDATE_NFLAG(dst);
-                                UPDATE_ZFLAG(dst);
-                                UPDATE_CFLAG_WITH_SC;
-//                              UPDATE_CFLAG(dst, lop, rop);
-                        }
-                        if (inst_cream->Rd == 15) {
-                                INC_PC(sizeof(orr_inst));
-                                goto DISPATCH;
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(orr_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        PKHBT_INST:
-        PKHTB_INST:
-        PLD_INST:
-        {
-                INC_ICOUNTER;
-                /* NOT IMPL */
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(stc_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        QADD_INST:
-        QADD16_INST:
-        QADD8_INST:
-        QADDSUBX_INST:
-        QDADD_INST:
-        QDSUB_INST:
-        QSUB_INST:
-        QSUB16_INST:
-        QSUB8_INST:
-        QSUBADDX_INST:
-        REV_INST:
-        {
-                INC_ICOUNTER;
-                rev_inst *inst_cream = (rev_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        RD = ((RM & 0xff) << 24) |
-                                (((RM >> 8) & 0xff) << 16) |
-                                (((RM >> 16) & 0xff) << 8) |
-                                ((RM >> 24) & 0xff);
-                        if (inst_cream->Rm == 15) {
-                                LOG_ERROR(Core_ARM11, "invalid operand for REV");
-                                CITRA_IGNORE_EXIT(-1);
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(rev_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        REV16_INST:
-        {
-                INC_ICOUNTER;
-                rev_inst *inst_cream = (rev_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        RD = (BITS(RM, 0, 7) << 8) | 
-                                BITS(RM, 8, 15) |
-                                (BITS(RM, 16, 23) << 24) |
-                                (BITS(RM, 24, 31) << 16);
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(rev_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        REVSH_INST:
-        RFE_INST:
-        RSB_INST:
-        {
-                INC_ICOUNTER;
-                rsb_inst *inst_cream = (rsb_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        rop = RN;
-                        lop = SHIFTER_OPERAND;
-                        if (inst_cream->Rn == 15) {
-                                rop += 2 * GET_INST_SIZE(cpu);;
-                        }
-                        RD = dst = lop - rop;
-                        if (inst_cream->S && (inst_cream->Rd == 15)) {
-                                /* cpsr = spsr */
-                                if (CurrentModeHasSPSR) {
-                                        cpu->Cpsr = cpu->Spsr_copy;
-                                        switch_mode(cpu, cpu->Spsr_copy & 0x1f);
-                                        LOAD_NZCVT;
-                                }
-                        } else if (inst_cream->S) {
-                                UPDATE_NFLAG(dst);
-                                UPDATE_ZFLAG(dst);
-                                UPDATE_CFLAG_NOT_BORROW_FROM(lop, rop);
-//                              UPDATE_VFLAG((int)dst, (int)lop, (int)rop);
-                                UPDATE_VFLAG_OVERFLOW_FROM(dst, lop, rop);
-                        }
-                        if (inst_cream->Rd == 15) {
-                                INC_PC(sizeof(rsb_inst));
-                                goto DISPATCH;
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(rsb_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        RSC_INST:
-        {
-                INC_ICOUNTER;
-                rsc_inst *inst_cream = (rsc_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        //lop = RN + !cpu->CFlag;
-                        //rop = SHIFTER_OPERAND;
-                        //RD = dst = rop - lop;
-                        lop = RN;
-                        rop = SHIFTER_OPERAND;
-                        RD = dst = rop - lop - !cpu->CFlag;
-                        if (inst_cream->S && (inst_cream->Rd == 15)) {
-                                /* cpsr = spsr */
-                                if (CurrentModeHasSPSR) {
-                                        cpu->Cpsr = cpu->Spsr_copy;
-                                        switch_mode(cpu, cpu->Spsr_copy & 0x1f);
-                                        LOAD_NZCVT;
-                                }
-                        } else if (inst_cream->S) {
-                                UPDATE_NFLAG(dst);
-                                UPDATE_ZFLAG(dst);
-//                              UPDATE_CFLAG(dst, lop, rop);
-//                              UPDATE_CFLAG_NOT_BORROW_FROM(rop, lop);
-                                UPDATE_CFLAG_NOT_BORROW_FROM_FLAG(rop, lop, !cpu->CFlag);
-//                              cpu->CFlag = !((ISNEG(lop) && ISPOS(rop)) || (ISNEG(lop) && ISPOS(dst)) || (ISPOS(rop) && ISPOS(dst)));
-                                UPDATE_VFLAG_OVERFLOW_FROM((int)dst, (int)rop, (int)lop);
-                        }
-                        if (inst_cream->Rd == 15) {
-                                INC_PC(sizeof(rsc_inst));
-                                goto DISPATCH;
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(rsc_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        SADD16_INST:
-        SADD8_INST:
-        SADDSUBX_INST:
-        SBC_INST:
-        {
-                INC_ICOUNTER;
-                sbc_inst *inst_cream = (sbc_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        lop = SHIFTER_OPERAND + !cpu->CFlag;
-                        rop = RN;
-                        RD = dst = rop - lop;
-                        if (inst_cream->S && (inst_cream->Rd == 15)) {
-                                /* cpsr = spsr */
-                                if (CurrentModeHasSPSR) {
-                                        cpu->Cpsr = cpu->Spsr_copy;
-                                        switch_mode(cpu, cpu->Spsr_copy & 0x1f);
-                                        LOAD_NZCVT;
-                                }
-                        } else if (inst_cream->S) {
-                                UPDATE_NFLAG(dst);
-                                UPDATE_ZFLAG(dst);
-//                              UPDATE_CFLAG(dst, lop, rop);
-                                //UPDATE_CFLAG_NOT_BORROW_FROM(rop, lop);
-                                //rop = rop - !cpu->CFlag;
-                                if(rop >= !cpu->CFlag)
-                                        UPDATE_CFLAG_NOT_BORROW_FROM(rop - !cpu->CFlag, SHIFTER_OPERAND);
-                                else
-                                        UPDATE_CFLAG_NOT_BORROW_FROM(rop, !cpu->CFlag);
-//                              cpu->CFlag = !((ISNEG(lop) && ISPOS(rop)) || (ISNEG(lop) && ISPOS(dst)) || (ISPOS(rop) && ISPOS(dst)));
-                                UPDATE_VFLAG_OVERFLOW_FROM(dst, rop, lop);
-                        }
-                        if (inst_cream->Rd == 15) {
-                                INC_PC(sizeof(sbc_inst));
-                                goto DISPATCH;
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(sbc_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        SEL_INST:
-        SETEND_INST:
-        SHADD16_INST:
-        SHADD8_INST:
-        SHADDSUBX_INST:
-        SHSUB16_INST:
-        SHSUB8_INST:
-        SHSUBADDX_INST:
-        SMLA_INST:
-        {
-                INC_ICOUNTER;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        smla_inst *inst_cream = (smla_inst *)inst_base->component;
-                        int32_t operand1, operand2;
-                        if (inst_cream->x == 0)
-                                operand1 = (BIT(RM, 15)) ? (BITS(RM, 0, 15) | 0xffff0000) : BITS(RM, 0, 15);
-                        else
-                                operand1 = (BIT(RM, 31)) ? (BITS(RM, 16, 31) | 0xffff0000) : BITS(RM, 16, 31);
-
-                        if (inst_cream->y == 0)
-                                operand2 = (BIT(RS, 15)) ? (BITS(RS, 0, 15) | 0xffff0000) : BITS(RS, 0, 15);
-                        else
-                                operand2 = (BIT(RS, 31)) ? (BITS(RS, 16, 31) | 0xffff0000) : BITS(RS, 16, 31);
-                        RD = operand1 * operand2 + RN;
-                        //FIXME: UPDATE Q FLAGS
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(smla_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        SMLAD_INST:
-        {
-                INC_ICOUNTER;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        smlad_inst *inst_cream = (smlad_inst *)inst_base->component;
-                        long long int rm = cpu->Reg[inst_cream->Rm];
-                        long long int rn = cpu->Reg[inst_cream->Rn];
-                        long long int ra = cpu->Reg[inst_cream->Ra];
-                        /* see SMUAD */
-                        if(inst_cream->Ra == 15)
-                                CITRA_IGNORE_EXIT(-1);
-                        int operand2 = (inst_cream->m)? ROTATE_RIGHT_32(rm, 16):rm;
-                        
-                        int half_rn, half_operand2;
-                        half_rn = rn & 0xFFFF;
-                        half_rn = (half_rn & 0x8000)? (0xFFFF0000|half_rn) : half_rn;
-
-                        half_operand2 = operand2 & 0xFFFF;
-                        half_operand2 = (half_operand2 & 0x8000)? (0xFFFF0000|half_operand2) : half_operand2;
-                
-                        long long int product1 = half_rn * half_operand2;
-
-                        half_rn = (rn & 0xFFFF0000) >> 16;
-                        half_rn = (half_rn & 0x8000)? (0xFFFF0000|half_rn) : half_rn;
-
-                        half_operand2 = (operand2 & 0xFFFF0000) >> 16;
-                        half_operand2 = (half_operand2 & 0x8000)? (0xFFFF0000|half_operand2) : half_operand2;
-                
-                        long long int product2 = half_rn * half_operand2;
-
-                        long long int signed_ra = (ra & 0x80000000)? (0xFFFFFFFF00000000LL) | ra : ra;
-                        long long int result = product1 + product2 + signed_ra;
-                        cpu->Reg[inst_cream->Rd] = result & 0xFFFFFFFF;
-                        /* FIXME , should check Signed overflow */
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(umlal_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-
-        SMLAL_INST:
-        {
-                INC_ICOUNTER;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        umlal_inst *inst_cream = (umlal_inst *)inst_base->component;
-                        long long int rm = RM;
-                        long long int rs = RS;
-                        if (BIT(rm, 31)) {
-                                rm |= 0xffffffff00000000LL;
-                        }
-                        if (BIT(rs, 31)) {
-                                rs |= 0xffffffff00000000LL;
-                        }
-                        long long int rst = rm * rs;
-                        long long int rdhi32 = RDHI;
-                        long long int hilo = (rdhi32 << 32) + RDLO;
-                        rst += hilo;
-                        RDLO = BITS(rst,  0, 31);
-                        RDHI = BITS(rst, 32, 63);
-                        if (inst_cream->S) {
-                                cpu->NFlag = BIT(RDHI, 31);
-                                cpu->ZFlag = (RDHI == 0 && RDLO == 0);
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(umlal_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        SMLALXY_INST:
-        SMLALD_INST:
-        SMLAW_INST:
-        SMLSD_INST:
-        SMLSLD_INST:
-        SMMLA_INST:
-        SMMLS_INST:
-        SMMUL_INST:
-        SMUAD_INST:
-        SMUL_INST:
-        {
-                INC_ICOUNTER;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        smul_inst *inst_cream = (smul_inst *)inst_base->component;
-                        uint32_t operand1, operand2;
-                        if (inst_cream->x == 0)
-                                operand1 = (BIT(RM, 15)) ? (BITS(RM, 0, 15) | 0xffff0000) : BITS(RM, 0, 15);
-                        else
-                                operand1 = (BIT(RM, 31)) ? (BITS(RM, 16, 31) | 0xffff0000) : BITS(RM, 16, 31);
-
-                        if (inst_cream->y == 0)
-                                operand2 = (BIT(RS, 15)) ? (BITS(RS, 0, 15) | 0xffff0000) : BITS(RS, 0, 15);
-                        else
-                                operand2 = (BIT(RS, 31)) ? (BITS(RS, 16, 31) | 0xffff0000) : BITS(RS, 16, 31);
-                        RD = operand1 * operand2;
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(smul_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        SMULL_INST:
-        {
-                INC_ICOUNTER;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        umull_inst *inst_cream = (umull_inst *)inst_base->component;
-//                      DEBUG_LOG(ARM11, "rm : [%llx] rs : [%llx] rst [%llx]\n", RM, RS, rst);
-                        int64_t rm = RM;
-                        int64_t rs = RS;
-                        if (BIT(rm, 31)) {
-                                rm |= 0xffffffff00000000LL;
-                        }
-                        if (BIT(rs, 31)) {
-                                rs |= 0xffffffff00000000LL;
-                        }
-                        int64_t rst = rm * rs;
-                        RDHI = BITS(rst, 32, 63);
-                        RDLO = BITS(rst,  0, 31);
-
-
-                        if (inst_cream->S) {
-                                cpu->NFlag = BIT(RDHI, 31);
-                                cpu->ZFlag = (RDHI == 0 && RDLO == 0);
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(umull_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        SMULW_INST:
-                INC_ICOUNTER;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        smlad_inst *inst_cream = (smlad_inst *)inst_base->component;
-//                      DEBUG_LOG(ARM11, "rm : [%llx] rs : [%llx] rst [%llx]\n", RM, RS, rst);
-                        int64_t rm = RM;
-                        int64_t rn = RN;
-                        if (inst_cream->m)
-                                rm = BITS(rm,16 , 31);
-                        else
-                                rm = BITS(rm,0 , 15);
-                        int64_t rst = rm * rn;
-                        RD = BITS(rst,  16, 47);
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(smlad_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-
-        SMUSD_INST:
-        SRS_INST:
-        SSAT_INST:
-        SSAT16_INST:
-        SSUB16_INST:
-        SSUB8_INST:
-        SSUBADDX_INST:
-        STC_INST:
-        {
-                INC_ICOUNTER;
-                /* NOT IMPL */
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(stc_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        STM_INST:
-        {
-                INC_ICOUNTER;
-                ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
-                unsigned int inst = inst_cream->inst;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        int i;
-                        unsigned int Rn = BITS(inst, 16, 19);
-                        unsigned int old_RN = cpu->Reg[Rn];
-
-                        fault = inst_cream->get_addr(cpu, inst_cream->inst, addr, phys_addr, 0);
-                        if (fault) goto MMU_EXCEPTION;
-                        if (BIT(inst_cream->inst, 22) == 1) {
-//                              DEBUG_MSG;
-                                #if 1
-                                for (i = 0; i < 13; i++) {
-                                        if(BIT(inst_cream->inst, i)){
-                                                fault = check_address_validity(cpu, addr, &phys_addr, 0);
-                                                if (fault) {
-                                                        goto MMU_EXCEPTION;
-                                                }
-                                                fault = interpreter_write_memory(addr, phys_addr, cpu->Reg[i], 32);
-                                                if (fault) goto MMU_EXCEPTION;
-                                                addr += 4;
-                                                phys_addr += 4;
-                                        }
-                                }
-                                if (BIT(inst_cream->inst, 13)) {
-                                        if (cpu->Mode == USER32MODE) {
-                                                fault = check_address_validity(cpu, addr, &phys_addr, 0);
-                                                if (fault) {
-                                                        goto MMU_EXCEPTION;
-                                                }
-                                                fault = interpreter_write_memory(addr, phys_addr, cpu->Reg[i], 32);
-                                                if (fault) goto MMU_EXCEPTION;
-                                                addr += 4;
-                                                phys_addr += 4;
-                                        } else {
-                                                fault = interpreter_write_memory(addr, phys_addr, cpu->Reg_usr[0], 32);
-                                                if (fault) goto MMU_EXCEPTION;
-                                                addr += 4;
-                                                phys_addr += 4;
-                                        }
-                                }
-                                if (BIT(inst_cream->inst, 14)) {
-                                        if (cpu->Mode == USER32MODE) {
-                                                fault = check_address_validity(cpu, addr, &phys_addr, 0);
-                                                if (fault) {
-                                                        goto MMU_EXCEPTION;
-                                                }
-                                                fault = interpreter_write_memory(addr, phys_addr, cpu->Reg[i], 32);
-                                                if (fault) goto MMU_EXCEPTION;
-                                                addr += 4;
-                                                phys_addr += 4;
-                                        } else {
-                                                fault = check_address_validity(cpu, addr, &phys_addr, 0);
-                                                if (fault) {
-                                                        goto MMU_EXCEPTION;
-                                                }
-                                                fault = interpreter_write_memory(addr, phys_addr, cpu->Reg_usr[1], 32);
-                                                if (fault) goto MMU_EXCEPTION;
-                                                addr += 4;
-                                                phys_addr += 4;
-                                        }
-                                }
-                                if (BIT(inst_cream->inst, 15)) {
-                                        fault = check_address_validity(cpu, addr, &phys_addr, 0);
-                                        if (fault) {
-                                                goto MMU_EXCEPTION;
-                                        }
-                                        fault = interpreter_write_memory(addr, phys_addr, cpu->Reg[i] + 8, 32);
-                                        if (fault) goto MMU_EXCEPTION;
-                                }
-                                #endif
-                        } else {
-                                for( i = 0; i < 15; i ++ ){
-                                        if(BIT(inst_cream->inst, i)){
-                                                //arch_write_memory(cpu, bb, Addr, R(i), 32);
-                                                //bus_write(32, addr, cpu->Reg[i]);
-                                                fault = check_address_validity(cpu, addr, &phys_addr, 0);
-                                                if (fault) {
-                                                        goto MMU_EXCEPTION;
-                                                }
-                                                if(i == Rn)
-                                                        fault = interpreter_write_memory(addr, phys_addr, old_RN, 32);
-                                                else
-                                                        fault = interpreter_write_memory(addr, phys_addr, cpu->Reg[i], 32);
-                                                if (fault) goto MMU_EXCEPTION;
-                                                addr += 4;
-                                                phys_addr += 4;
-                                                //Addr = ADD(Addr, CONST(4));
-                                        }
-                                }
-
-                                /* check pc reg*/
-                                if(BIT(inst_cream->inst, i)){
-                                        //arch_write_memory(cpu, bb, Addr, STOREM_CHECK_PC, 32);
-                                        //bus_write(32, addr, cpu->Reg[i] + 8);
-                                        fault = check_address_validity(cpu, addr, &phys_addr, 0);
-                                        if (fault) {
-                                                goto MMU_EXCEPTION;
-                                        }
-                                        fault = interpreter_write_memory(addr, phys_addr, cpu->Reg[i] + 8, 32);
-                                        if (fault) goto MMU_EXCEPTION;
-                                }
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(ldst_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        SXTB_INST:
-        {
-                INC_ICOUNTER;
-                sxtb_inst *inst_cream = (sxtb_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        if (inst_cream->Rm == 15) {
-                                LOG_ERROR(Core_ARM11, "invalid operand for SXTB");
-                                CITRA_IGNORE_EXIT(-1);
-                        }
-                        unsigned int operand2 = ROTATE_RIGHT_32(RM, 8 * inst_cream->rotate);
-                        if (BIT(operand2, 7)) {
-                                operand2 |= 0xffffff00;
-                        } else
-                                operand2 &= 0xff;
-                        RD = operand2;
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(sxtb_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        STR_INST:
-        {
-                INC_ICOUNTER;
-                ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        fault = inst_cream->get_addr(cpu, inst_cream->inst, addr, phys_addr, 0);
-                        if (fault) goto MMU_EXCEPTION;
-                        unsigned int value = cpu->Reg[BITS(inst_cream->inst, 12, 15)];
-                        //bus_write(32, addr, value);
-                        fault = interpreter_write_memory(addr, phys_addr, value, 32);
-                        if (fault) goto MMU_EXCEPTION;
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(ldst_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        UXTB_INST:
-        {
-                INC_ICOUNTER;
-                uxtb_inst *inst_cream = (uxtb_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        unsigned int operand2 = ROTATE_RIGHT_32(RM, 8 * inst_cream->rotate) 
-                                                & 0xff;
-                        RD = operand2;
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(uxtb_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        UXTAB_INST:
-        {
-                INC_ICOUNTER;
-                uxtab_inst *inst_cream = (uxtab_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        unsigned int operand2 = ROTATE_RIGHT_32(RM, 8 * inst_cream->rotate) 
-                                                & 0xff;
-                        RD = RN + operand2;
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(uxtab_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        STRB_INST:
-        {
-                INC_ICOUNTER;
-                ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        fault = inst_cream->get_addr(cpu, inst_cream->inst, addr, phys_addr, 0);
-                        if (fault) goto MMU_EXCEPTION;
-                        unsigned int value = cpu->Reg[BITS(inst_cream->inst, 12, 15)] & 0xff;
-                        //bus_write(8, addr, value);
-                        fault = interpreter_write_memory(addr, phys_addr, value, 8);
-                        if (fault) goto MMU_EXCEPTION;
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(ldst_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        STRBT_INST:
-        {
-                INC_ICOUNTER;
-                ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        fault = inst_cream->get_addr(cpu, inst_cream->inst, addr, phys_addr, 0);
-                        if (fault) goto MMU_EXCEPTION;
-                        unsigned int value = cpu->Reg[BITS(inst_cream->inst, 12, 15)] & 0xff;
-                        //bus_write(8, addr, value);
-                        fault = interpreter_write_memory(addr, phys_addr, value, 8);
-                        if (fault) goto MMU_EXCEPTION;
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                //if (BITS(inst_cream->inst, 12, 15) == 15)
-                //      goto DISPATCH;
-                INC_PC(sizeof(ldst_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        STRD_INST:
-        {
-                INC_ICOUNTER;
-                ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        fault = inst_cream->get_addr(cpu, inst_cream->inst, addr, phys_addr, 0);
-                        if (fault) goto MMU_EXCEPTION;
-                        uint32_t rear_phys_addr;
-                        fault = check_address_validity(cpu, addr + 4, &rear_phys_addr, 0);
+                        cpu->Reg[i] = ret;
+                        addr += 4;
+                        if ((addr & 0xfff) == 0) {
+                            fault = check_address_validity(cpu, addr, &phys_addr, 1);
+                        } else {
+                            phys_addr += 4;
+                        }
+                    }
+                }
+            } else if (BIT(inst, 22) && BIT(inst, 15)) {
+                for( i = 0; i < 15; i ++ ){
+                    if(BIT(inst, i)){
+                        fault = interpreter_read_memory(addr, phys_addr, ret, 32);
+                        cpu->Reg[i] = ret;
+                        addr += 4;
+                        if ((addr & 0xfff) == 0) {
+                            fault = check_address_validity(cpu, addr, &phys_addr, 1);
+                        } else {
+                            phys_addr += 4;
+                        }
+                     }
+                 }
+                
+                if (CurrentModeHasSPSR) {
+                    cpu->Cpsr = cpu->Spsr_copy;
+                    switch_mode(cpu, cpu->Cpsr & 0x1f);
+                    LOAD_NZCVT;
+                }
+
+                fault = interpreter_read_memory(addr, phys_addr, ret, 32);
+                if (fault) {
+                    goto MMU_EXCEPTION;
+                }
+                cpu->Reg[15] = ret;
+             }
+            if (BIT(inst, 15)) {
+                INC_PC(sizeof(ldst_inst));
+                goto DISPATCH;
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(ldst_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    SXTH_INST:
+    {
+        sxth_inst *inst_cream = (sxth_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            unsigned int operand2 = ROTATE_RIGHT_32(RM, 8 * inst_cream->rotate);
+            if (BIT(operand2, 15)) {
+                operand2 |= 0xffff0000;
+            } else {
+                operand2 &= 0xffff;
+            }
+            RD = operand2;
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(sxth_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    LDR_INST:
+    {
+        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+        //if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            fault = inst_cream->get_addr(cpu, inst_cream->inst, addr, phys_addr, 1);
+            if (fault) goto MMU_EXCEPTION;
+            unsigned int value;
+            fault = interpreter_read_memory(addr, phys_addr, value, 32);
+            if (BIT(CP15_REG(CP15_CONTROL), 22) == 1)
+                cpu->Reg[BITS(inst_cream->inst, 12, 15)] = value;
+            else {
+                value = ROTATE_RIGHT_32(value,(8*(addr&0x3)));
+                cpu->Reg[BITS(inst_cream->inst, 12, 15)] = value;
+            }
+            if (BITS(inst_cream->inst, 12, 15) == 15) {
+                // For armv5t, should enter thumb when bits[0] is non-zero.
+                cpu->TFlag = value & 0x1;
+                cpu->Reg[15] &= 0xFFFFFFFE;
+                INC_PC(sizeof(ldst_inst));
+                goto DISPATCH;
+            }
+        //}
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(ldst_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    LDRCOND_INST:
+    {
+        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+        if (CondPassed(cpu, inst_base->cond)) {
+            fault = inst_cream->get_addr(cpu, inst_cream->inst, addr, phys_addr, 1);
+            if (fault) goto MMU_EXCEPTION;
+            unsigned int value;
+            fault = interpreter_read_memory(addr, phys_addr, value, 32);
+            if (BIT(CP15_REG(CP15_CONTROL), 22) == 1)
+                cpu->Reg[BITS(inst_cream->inst, 12, 15)] = value;
+            else {
+                value = ROTATE_RIGHT_32(value,(8*(addr&0x3)));
+                cpu->Reg[BITS(inst_cream->inst, 12, 15)] = value;
+            }
+
+            if (BITS(inst_cream->inst, 12, 15) == 15) {
+                // For armv5t, should enter thumb when bits[0] is non-zero.
+                cpu->TFlag = value & 0x1;
+                cpu->Reg[15] &= 0xFFFFFFFE;
+                INC_PC(sizeof(ldst_inst));
+                goto DISPATCH;
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(ldst_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    UXTH_INST:
+    {
+        uxth_inst *inst_cream = (uxth_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            unsigned int operand2 = ROTATE_RIGHT_32(RM, 8 * inst_cream->rotate) 
+                        & 0xffff;
+            RD = operand2;
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(uxth_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    UXTAH_INST:
+    {
+        uxtah_inst *inst_cream = (uxtah_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            unsigned int operand2 = ROTATE_RIGHT_32(RM, 8 * inst_cream->rotate) 
+                        & 0xffff;
+            RD = RN + operand2;
+            if (inst_cream->Rn == 15 || inst_cream->Rm == 15) {
+                LOG_ERROR(Core_ARM11, "invalid operands for UXTAH");
+                CITRA_IGNORE_EXIT(-1);
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(uxtah_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    LDRB_INST:
+    {
+        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            fault = inst_cream->get_addr(cpu, inst_cream->inst, addr, phys_addr, 1);
+            if (fault) goto MMU_EXCEPTION;
+            unsigned int value;
+            fault = interpreter_read_memory(addr, phys_addr, value, 8);
+            if (fault) goto MMU_EXCEPTION;
+            cpu->Reg[BITS(inst_cream->inst, 12, 15)] = value;
+            if (BITS(inst_cream->inst, 12, 15) == 15) {
+                INC_PC(sizeof(ldst_inst));
+                goto DISPATCH;
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(ldst_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    LDRBT_INST:
+    {
+        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            fault = inst_cream->get_addr(cpu, inst_cream->inst, addr, phys_addr, 1);
+            if (fault) goto MMU_EXCEPTION;
+            unsigned int value;
+            fault = interpreter_read_memory(addr, phys_addr, value, 8);
+            if (fault) goto MMU_EXCEPTION;
+            cpu->Reg[BITS(inst_cream->inst, 12, 15)] = value;
+            if (BITS(inst_cream->inst, 12, 15) == 15) {
+                INC_PC(sizeof(ldst_inst));
+                goto DISPATCH;
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(ldst_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    LDRD_INST:
+    {
+        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            // Should check if RD is even-numbered, Rd != 14, addr[0:1] == 0, (CP15_reg1_U == 1 || addr[2] == 0)
+            fault = inst_cream->get_addr(cpu, inst_cream->inst, addr, phys_addr, 1);
+            if (fault) goto MMU_EXCEPTION;
+            uint32_t rear_phys_addr;
+             fault = check_address_validity(cpu, addr + 4, &rear_phys_addr, 1);
+            if(fault){
+                LOG_ERROR(Core_ARM11, "MMU fault , should rollback the above get_addr\n");
+                CITRA_IGNORE_EXIT(-1);
+                goto MMU_EXCEPTION;
+            }
+            unsigned int value;
+            fault = interpreter_read_memory(addr, phys_addr, value, 32);
+            if (fault) goto MMU_EXCEPTION;
+            cpu->Reg[BITS(inst_cream->inst, 12, 15)] = value;
+            fault = interpreter_read_memory(addr + 4, rear_phys_addr, value, 32);
+            if (fault) goto MMU_EXCEPTION;
+            cpu->Reg[BITS(inst_cream->inst, 12, 15) + 1] = value;
+
+            // No dispatch since this operation should not modify R15
+        }
+        cpu->Reg[15] += 4;
+        INC_PC(sizeof(ldst_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+
+    LDREX_INST:
+    {
+        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            addr = cpu->Reg[BITS(inst_cream->inst, 16, 19)];
+            fault = check_address_validity(cpu, addr, &phys_addr, 1);
+            if (fault) goto MMU_EXCEPTION;
+            unsigned int value;
+            fault = interpreter_read_memory(addr, phys_addr, value, 32);
+            if (fault) goto MMU_EXCEPTION;
+
+            add_exclusive_addr(cpu, phys_addr);
+            cpu->exclusive_state = 1;
+
+            cpu->Reg[BITS(inst_cream->inst, 12, 15)] = value;
+            if (BITS(inst_cream->inst, 12, 15) == 15) {
+                INC_PC(sizeof(ldst_inst));
+                goto DISPATCH;
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(ldst_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    LDREXB_INST:
+    {
+        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            addr = cpu->Reg[BITS(inst_cream->inst, 16, 19)];
+            fault = check_address_validity(cpu, addr, &phys_addr, 1);
+            if (fault) goto MMU_EXCEPTION;
+            unsigned int value;
+            fault = interpreter_read_memory(addr, phys_addr, value, 8);
+            if (fault) goto MMU_EXCEPTION;
+
+            add_exclusive_addr(cpu, phys_addr);
+            cpu->exclusive_state = 1;
+
+            cpu->Reg[BITS(inst_cream->inst, 12, 15)] = value;
+            if (BITS(inst_cream->inst, 12, 15) == 15) {
+                INC_PC(sizeof(ldst_inst));
+                goto DISPATCH;
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(ldst_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    LDRH_INST:
+    {
+        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            fault = inst_cream->get_addr(cpu, inst_cream->inst, addr, phys_addr, 1);
+            if (fault) goto MMU_EXCEPTION;
+            unsigned int value = 0;
+            fault = interpreter_read_memory(addr, phys_addr, value, 16);
+            if (fault) goto MMU_EXCEPTION;
+            cpu->Reg[BITS(inst_cream->inst, 12, 15)] = value;
+            if (BITS(inst_cream->inst, 12, 15) == 15) {
+                INC_PC(sizeof(ldst_inst));
+                goto DISPATCH;
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(ldst_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    LDRSB_INST:
+    {
+        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            fault = inst_cream->get_addr(cpu, inst_cream->inst, addr, phys_addr, 1);
+            if (fault) goto MMU_EXCEPTION;
+            unsigned int value;
+            fault = interpreter_read_memory(addr, phys_addr, value, 8);
+            if (fault) goto MMU_EXCEPTION;
+            if (BIT(value, 7)) {
+                value |= 0xffffff00;
+            }
+            cpu->Reg[BITS(inst_cream->inst, 12, 15)] = value;
+            if (BITS(inst_cream->inst, 12, 15) == 15) {
+                INC_PC(sizeof(ldst_inst));
+                goto DISPATCH;
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(ldst_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    LDRSH_INST:
+    {
+        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            fault = inst_cream->get_addr(cpu, inst_cream->inst, addr, phys_addr, 1);
+            if (fault) goto MMU_EXCEPTION;
+            unsigned int value;
+            fault = interpreter_read_memory(addr, phys_addr, value, 16);
+            if (fault) goto MMU_EXCEPTION;
+            if (BIT(value, 15)) {
+                value |= 0xffff0000;
+            }
+            cpu->Reg[BITS(inst_cream->inst, 12, 15)] = value;
+            if (BITS(inst_cream->inst, 12, 15) == 15) {
+                INC_PC(sizeof(ldst_inst));
+                goto DISPATCH;
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(ldst_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    LDRT_INST:
+    {
+        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            fault = inst_cream->get_addr(cpu, inst_cream->inst, addr, phys_addr, 1);
+            if (fault) goto MMU_EXCEPTION;
+            unsigned int value;
+            fault = interpreter_read_memory(addr, phys_addr, value, 32);
+            if (fault) goto MMU_EXCEPTION;
+            cpu->Reg[BITS(inst_cream->inst, 12, 15)] = value;
+
+            if (BIT(CP15_REG(CP15_CONTROL), 22) == 1)
+                cpu->Reg[BITS(inst_cream->inst, 12, 15)] = value;
+            else
+                cpu->Reg[BITS(inst_cream->inst, 12, 15)] = ROTATE_RIGHT_32(value,(8*(addr&0x3))) ;
+
+            if (BITS(inst_cream->inst, 12, 15) == 15) {
+                INC_PC(sizeof(ldst_inst));
+                goto DISPATCH;
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(ldst_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    MCR_INST:
+    {
+        mcr_inst *inst_cream = (mcr_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            unsigned int inst = inst_cream->inst;
+            if (inst_cream->Rd == 15) {
+                DEBUG_MSG;
+            } else {
+                if (inst_cream->cp_num == 15) {
+                    if(CRn == 0 && OPCODE_2 == 0 && CRm == 0) {
+                        CP15_REG(CP15_MAIN_ID) = RD;
+                    } else if (CRn == 1 && CRm == 0 && OPCODE_2 == 1) {
+                        CP15_REG(CP15_AUXILIARY_CONTROL) = RD;
+                    } else if (CRn == 1 && CRm == 0 && OPCODE_2 == 2) {
+                        CP15_REG(CP15_COPROCESSOR_ACCESS_CONTROL) = RD;
+                    } else if(CRn == 1 && CRm == 0 && OPCODE_2 == 0) {
+                        CP15_REG(CP15_CONTROL) = RD;
+                    } else if (CRn == 3 && CRm == 0 && OPCODE_2 == 0) {
+                        CP15_REG(CP15_DOMAIN_ACCESS_CONTROL) = RD;
+                    } else if (CRn == 2 && CRm == 0 && OPCODE_2 == 0) {
+                        CP15_REG(CP15_TRANSLATION_BASE_TABLE_0) = RD;
+                    } else if (CRn == 2 && CRm == 0 && OPCODE_2 == 1) {
+                        CP15_REG(CP15_TRANSLATION_BASE_TABLE_1) = RD;
+                    } else if (CRn == 2 && CRm == 0 && OPCODE_2 == 2) {
+                        CP15_REG(CP15_TRANSLATION_BASE_CONTROL) = RD;
+                    } else if(CRn == MMU_CACHE_OPS){
+                        //LOG_WARNING(Core_ARM11, "cache operations have not implemented.");
+                    } else if(CRn == MMU_TLB_OPS){
+                        switch (CRm) {
+                        case 5: // ITLB
+                            switch(OPCODE_2) {
+                            case 0: // Invalidate all
+                                LOG_DEBUG(Core_ARM11, "{TLB} [INSN] invalidate all");
+                                break;
+                            case 1: // Invalidate by MVA
+                                LOG_DEBUG(Core_ARM11, "{TLB} [INSN] invalidate by mva");
+                                break;
+                            case 2: // Invalidate by asid
+                                LOG_DEBUG(Core_ARM11, "{TLB} [INSN] invalidate by asid");
+                                break;
+                            default:
+                                break;
+                            }
+
+                            break;
+                        case 6: // DTLB
+                            switch(OPCODE_2){
+                            case 0: // Invalidate all
+                                LOG_DEBUG(Core_ARM11, "{TLB} [DATA] invalidate all");
+                                break;
+                            case 1: // Invalidate by MVA
+                                LOG_DEBUG(Core_ARM11, "{TLB} [DATA] invalidate by mva");
+                                break;
+                            case 2: // Invalidate by asid
+                                LOG_DEBUG(Core_ARM11, "{TLB} [DATA] invalidate by asid");
+                                break;
+                            default:
+                                break;
+                            }
+                            break;
+                        case 7: // UNIFILED TLB
+                            switch(OPCODE_2){
+                            case 0: // invalidate all
+                                LOG_DEBUG(Core_ARM11, "{TLB} [UNIFILED] invalidate all");
+                                break;
+                            case 1: // Invalidate by MVA
+                                LOG_DEBUG(Core_ARM11, "{TLB} [UNIFILED] invalidate by mva");
+                                break;
+                            case 2: // Invalidate by asid
+                                LOG_DEBUG(Core_ARM11, "{TLB} [UNIFILED] invalidate by asid");
+                                break;
+                            default:
+                                break;
+                            }
+                            break;
+                        default:
+                            break;
+                        }
+                    } else if(CRn == MMU_PID) {
+                        if(OPCODE_2 == 0)
+                            CP15_REG(CP15_PID) = RD;
+                        else if(OPCODE_2 == 1)
+                            CP15_REG(CP15_CONTEXT_ID) = RD;
+                        else if(OPCODE_2 == 3) {
+                            CP15_REG(CP15_THREAD_URO) = RD;
+                        } else {
+                            LOG_ERROR(Core_ARM11, "mmu_mcr wrote UNKNOWN - reg %d", CRn);
+                        }
+                    } else {
+                        LOG_ERROR(Core_ARM11, "mcr CRn=%d, CRm=%d OP2=%d is not implemented", CRn, CRm, OPCODE_2);
+                    }
+                }
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(mcr_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    MCRR_INST:
+    MLA_INST:
+    {
+        mla_inst *inst_cream = (mla_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            uint64_t rm = RM;
+            uint64_t rs = RS;
+            uint64_t rn = RN;
+            if (inst_cream->Rm == 15 || inst_cream->Rs == 15 || inst_cream->Rn == 15) {
+                LOG_ERROR(Core_ARM11, "invalid operands for MLA");
+                CITRA_IGNORE_EXIT(-1);
+            }
+            RD = dst = static_cast<uint32_t>((rm * rs + rn) & 0xffffffff);
+            if (inst_cream->S) {
+                UPDATE_NFLAG(dst);
+                UPDATE_ZFLAG(dst);
+            }
+            if (inst_cream->Rd == 15) {
+                INC_PC(sizeof(mla_inst));
+                goto DISPATCH;
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(mla_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    MOV_INST:
+    {
+        mov_inst *inst_cream = (mov_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            RD = dst = SHIFTER_OPERAND;
+            if (inst_cream->S && (inst_cream->Rd == 15)) {
+                if (CurrentModeHasSPSR) {
+                    cpu->Cpsr = cpu->Spsr_copy;
+                    switch_mode(cpu, cpu->Spsr_copy & 0x1f);
+                    LOAD_NZCVT;
+                }
+            } else if (inst_cream->S) {
+                UPDATE_NFLAG(dst);
+                UPDATE_ZFLAG(dst);
+                UPDATE_CFLAG_WITH_SC;
+            }
+            if (inst_cream->Rd == 15) {
+                INC_PC(sizeof(mov_inst));
+                goto DISPATCH;
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(mov_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    MRC_INST:
+    {
+        mrc_inst *inst_cream = (mrc_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            unsigned int inst = inst_cream->inst;
+            if (inst_cream->Rd == 15) {
+                DEBUG_MSG;
+            }
+            if (inst_cream->inst == 0xeef04a10) {
+                // Undefined instruction fmrx
+                RD = 0x20000000;
+                CITRA_IGNORE_EXIT(-1);
+                goto END;
+            } else {
+                if (inst_cream->cp_num == 15) {
+                    if(CRn == 0 && OPCODE_2 == 0 && CRm == 0) {
+                        RD = cpu->CP15[CP15(CP15_MAIN_ID)];
+                    } else if (CRn == 1 && CRm == 0 && OPCODE_2 == 0) {
+                        RD = cpu->CP15[CP15(CP15_CONTROL)];
+                    } else if (CRn == 1 && CRm == 0 && OPCODE_2 == 1) {
+                        RD = cpu->CP15[CP15(CP15_AUXILIARY_CONTROL)];
+                    } else if (CRn == 1 && CRm == 0 && OPCODE_2 == 2) {
+                        RD = cpu->CP15[CP15(CP15_COPROCESSOR_ACCESS_CONTROL)];
+                    } else if (CRn == 3 && CRm == 0 && OPCODE_2 == 0) {
+                        RD = cpu->CP15[CP15(CP15_DOMAIN_ACCESS_CONTROL)];
+                    } else if (CRn == 2 && CRm == 0 && OPCODE_2 == 0) {
+                        RD = cpu->CP15[CP15(CP15_TRANSLATION_BASE_TABLE_0)];
+                    } else if (CRn == 5 && CRm == 0 && OPCODE_2 == 0) {
+                        RD = cpu->CP15[CP15(CP15_FAULT_STATUS)];
+                    } else if (CRn == 6 && CRm == 0 && OPCODE_2 == 0) {
+                        RD = cpu->CP15[CP15(CP15_FAULT_ADDRESS)];
+                    } else if (CRn == 0 && CRm == 0 && OPCODE_2 == 1) {
+                        RD = cpu->CP15[CP15(CP15_CACHE_TYPE)];
+                    } else if (CRn == 5 && CRm == 0 && OPCODE_2 == 1) {
+                        RD = cpu->CP15[CP15(CP15_INSTR_FAULT_STATUS)];
+                    } else if (CRn == 13) {
+                        if(OPCODE_2 == 0)
+                            RD = CP15_REG(CP15_PID);
+                        else if(OPCODE_2 == 1)
+                            RD = CP15_REG(CP15_CONTEXT_ID);
+                        else if(OPCODE_2 == 3) {
+                            RD = Memory::KERNEL_MEMORY_VADDR;
+                        } else {
+                            LOG_ERROR(Core_ARM11, "mmu_mrr wrote UNKNOWN - reg %d", CRn);
+                        }
+                    } else {
+                        LOG_ERROR(Core_ARM11, "mrc CRn=%d, CRm=%d, OP2=%d is not implemented", CRn, CRm, OPCODE_2);
+                    }
+                }
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(mrc_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    MRRC_INST:
+    MRS_INST:
+    {
+        mrs_inst *inst_cream = (mrs_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            if (inst_cream->R) {
+                RD = cpu->Spsr_copy;
+            } else {
+                SAVE_NZCVT;
+                RD = cpu->Cpsr;
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(mrs_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    MSR_INST:
+    {
+        msr_inst *inst_cream = (msr_inst *)inst_base->component;
+        const uint32_t UnallocMask = 0x06f0fc00, UserMask = 0xf80f0200, PrivMask = 0x000001df, StateMask = 0x01000020;
+        unsigned int inst = inst_cream->inst;
+        unsigned int operand;
+
+        if (BIT(inst, 25)) {
+            int rot_imm = BITS(inst, 8, 11) * 2;
+            operand = ROTATE_RIGHT_32(BITS(inst, 0, 7), rot_imm);
+        } else {
+            operand = cpu->Reg[BITS(inst, 0, 3)];
+        }
+        uint32_t byte_mask = (BIT(inst, 16) ? 0xff : 0) | (BIT(inst, 17) ? 0xff00 : 0)
+                    | (BIT(inst, 18) ? 0xff0000 : 0) | (BIT(inst, 19) ? 0xff000000 : 0);
+        uint32_t mask;
+        if (!inst_cream->R) {
+            if (InAPrivilegedMode(cpu)) {
+                if ((operand & StateMask) != 0) {
+                    /// UNPREDICTABLE
+                    DEBUG_MSG;
+                } else
+                    mask = byte_mask & (UserMask | PrivMask);
+            } else {
+                mask = byte_mask & UserMask;
+            }
+            SAVE_NZCVT;
+
+            cpu->Cpsr = (cpu->Cpsr & ~mask) | (operand & mask);
+            switch_mode(cpu, cpu->Cpsr & 0x1f);
+            LOAD_NZCVT;
+        } else {
+            if (CurrentModeHasSPSR) {
+                mask = byte_mask & (UserMask | PrivMask | StateMask);
+                cpu->Spsr_copy = (cpu->Spsr_copy & ~mask) | (operand & mask);
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(msr_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    MUL_INST:
+    {
+        mul_inst *inst_cream = (mul_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            uint64_t rm = RM;
+            uint64_t rs = RS;
+            RD = dst = static_cast<uint32_t>((rm * rs) & 0xffffffff);
+            if (inst_cream->S) {
+                UPDATE_NFLAG(dst);
+                UPDATE_ZFLAG(dst);
+            }
+            if (inst_cream->Rd == 15) {
+                INC_PC(sizeof(mul_inst));
+                goto DISPATCH;
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(mul_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    MVN_INST:
+    {
+        mvn_inst *inst_cream = (mvn_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            RD = dst = ~SHIFTER_OPERAND;
+            if (inst_cream->S && (inst_cream->Rd == 15)) {
+                if (CurrentModeHasSPSR) {
+                    cpu->Cpsr = cpu->Spsr_copy;
+                    switch_mode(cpu, cpu->Spsr_copy & 0x1f);
+                    LOAD_NZCVT;
+                }
+            } else if (inst_cream->S) {
+                UPDATE_NFLAG(dst);
+                UPDATE_ZFLAG(dst);
+                UPDATE_CFLAG_WITH_SC;
+            }
+            if (inst_cream->Rd == 15) {
+                INC_PC(sizeof(mvn_inst));
+                goto DISPATCH;
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(mvn_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    ORR_INST:
+    {
+        orr_inst *inst_cream = (orr_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            lop = RN;
+            rop = SHIFTER_OPERAND;
+            RD = dst = lop | rop;
+            if (inst_cream->S && (inst_cream->Rd == 15)) {
+                if (CurrentModeHasSPSR) {
+                    cpu->Cpsr = cpu->Spsr_copy;
+                    switch_mode(cpu, cpu->Spsr_copy & 0x1f);
+                    LOAD_NZCVT;
+                }
+            } else if (inst_cream->S) {
+                UPDATE_NFLAG(dst);
+                UPDATE_ZFLAG(dst);
+                UPDATE_CFLAG_WITH_SC;
+            }
+            if (inst_cream->Rd == 15) {
+                INC_PC(sizeof(orr_inst));
+                goto DISPATCH;
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(orr_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+
+    PKHBT_INST:
+    {
+        if (inst_base->cond == 0xE || CondPassed(cpu, inst_base->cond)) {
+            pkh_inst *inst_cream = (pkh_inst *)inst_base->component;
+            RD = (RN & 0xFFFF) | ((RM << inst_cream->imm) & 0xFFFF0000);
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(pkh_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+
+    PKHTB_INST:
+    {
+        if (inst_base->cond == 0xE || CondPassed(cpu, inst_base->cond)) {
+            pkh_inst *inst_cream = (pkh_inst *)inst_base->component;
+            int shift_imm = inst_cream->imm ? inst_cream->imm : 31;
+            RD = ((static_cast<s32>(RM) >> shift_imm) & 0xFFFF) | (RN & 0xFFFF0000);
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(pkh_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+
+    PLD_INST:
+    {
+        // Instruction not implemented
+        //LOG_CRITICAL(Core_ARM11, "unimplemented instruction");
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(stc_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+
+    QADD_INST:
+    QADD8_INST:
+    QADD16_INST:
+    QADDSUBX_INST:
+    QSUB8_INST:
+    QSUB16_INST:
+    QSUBADDX_INST:
+    {
+        if (inst_base->cond == 0xE || CondPassed(cpu, inst_base->cond)) {
+            generic_arm_inst* const inst_cream = (generic_arm_inst*)inst_base->component;
+            const u16 rm_lo = (RM & 0xFFFF);
+            const u16 rm_hi = ((RM >> 16) & 0xFFFF);
+            const u16 rn_lo = (RN & 0xFFFF);
+            const u16 rn_hi = ((RN >> 16) & 0xFFFF);
+            const u8 op2    = inst_cream->op2;
+
+            u16 lo_result = 0;
+            u16 hi_result = 0;
+
+            // QADD16
+            if (op2 == 0x00) {
+                lo_result = ARMul_SignedSaturatedAdd16(rn_lo, rm_lo);
+                hi_result = ARMul_SignedSaturatedAdd16(rn_hi, rm_hi);
+            }
+            // QASX
+            else if (op2 == 0x01) {
+                lo_result = ARMul_SignedSaturatedSub16(rn_lo, rm_hi);
+                hi_result = ARMul_SignedSaturatedAdd16(rn_hi, rm_lo);
+            }
+            // QSAX
+            else if (op2 == 0x02) {
+                lo_result = ARMul_SignedSaturatedAdd16(rn_lo, rm_hi);
+                hi_result = ARMul_SignedSaturatedSub16(rn_hi, rm_lo);
+            }
+            // QSUB16
+            else if (op2 == 0x03) {
+                lo_result = ARMul_SignedSaturatedSub16(rn_lo, rm_lo);
+                hi_result = ARMul_SignedSaturatedSub16(rn_hi, rm_hi);
+            }
+            // QADD8
+            else if (op2 == 0x04) {
+                lo_result = ARMul_SignedSaturatedAdd8(rn_lo & 0xFF, rm_lo & 0xFF) |
+                            ARMul_SignedSaturatedAdd8(rn_lo >> 8, rm_lo >> 8) << 8;
+                hi_result = ARMul_SignedSaturatedAdd8(rn_hi & 0xFF, rm_hi & 0xFF) |
+                            ARMul_SignedSaturatedAdd8(rn_hi >> 8, rm_hi >> 8) << 8;
+            }
+            // QSUB8
+            else if (op2 == 0x07) {
+                lo_result = ARMul_SignedSaturatedSub8(rn_lo & 0xFF, rm_lo & 0xFF) |
+                            ARMul_SignedSaturatedSub8(rn_lo >> 8, rm_lo >> 8) << 8;
+                hi_result = ARMul_SignedSaturatedSub8(rn_hi & 0xFF, rm_hi & 0xFF) |
+                            ARMul_SignedSaturatedSub8(rn_hi >> 8, rm_hi >> 8) << 8;
+            }
+
+            RD = (lo_result & 0xFFFF) | ((hi_result & 0xFFFF) << 16);
+        }
+
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(generic_arm_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+
+    QDADD_INST:
+    QDSUB_INST:
+    QSUB_INST:
+    REV_INST:
+    {
+        rev_inst *inst_cream = (rev_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            RD = ((RM & 0xff) << 24) |
+                (((RM >> 8) & 0xff) << 16) |
+                (((RM >> 16) & 0xff) << 8) |
+                ((RM >> 24) & 0xff);
+            if (inst_cream->Rm == 15) {
+                LOG_ERROR(Core_ARM11, "invalid operand for REV");
+                CITRA_IGNORE_EXIT(-1);
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(rev_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    REV16_INST:
+    {
+        rev_inst *inst_cream = (rev_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            RD = (BITS(RM, 0, 7) << 8) | 
+                BITS(RM, 8, 15) |
+                (BITS(RM, 16, 23) << 24) |
+                (BITS(RM, 24, 31) << 16);
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(rev_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    REVSH_INST:
+    RFE_INST:
+    RSB_INST:
+    {
+        rsb_inst *inst_cream = (rsb_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            rop = RN;
+            lop = SHIFTER_OPERAND;
+            if (inst_cream->Rn == 15) {
+                rop += 2 * GET_INST_SIZE(cpu);;
+            }
+            RD = dst = lop - rop;
+            if (inst_cream->S && (inst_cream->Rd == 15)) {
+                if (CurrentModeHasSPSR) {
+                    cpu->Cpsr = cpu->Spsr_copy;
+                    switch_mode(cpu, cpu->Spsr_copy & 0x1f);
+                    LOAD_NZCVT;
+                }
+            } else if (inst_cream->S) {
+                UPDATE_NFLAG(dst);
+                UPDATE_ZFLAG(dst);
+                UPDATE_CFLAG_NOT_BORROW_FROM(lop, rop);
+                UPDATE_VFLAG_OVERFLOW_FROM(dst, lop, rop);
+            }
+            if (inst_cream->Rd == 15) {
+                INC_PC(sizeof(rsb_inst));
+                goto DISPATCH;
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(rsb_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    RSC_INST:
+    {
+        rsc_inst *inst_cream = (rsc_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            lop = RN;
+            rop = SHIFTER_OPERAND;
+            RD = dst = rop - lop - !cpu->CFlag;
+            if (inst_cream->S && (inst_cream->Rd == 15)) {
+                if (CurrentModeHasSPSR) {
+                    cpu->Cpsr = cpu->Spsr_copy;
+                    switch_mode(cpu, cpu->Spsr_copy & 0x1f);
+                    LOAD_NZCVT;
+                }
+            } else if (inst_cream->S) {
+                UPDATE_NFLAG(dst);
+                UPDATE_ZFLAG(dst);
+                UPDATE_CFLAG_NOT_BORROW_FROM_FLAG(rop, lop, !cpu->CFlag);
+                UPDATE_VFLAG_OVERFLOW_FROM((int)dst, (int)rop, (int)lop);
+            }
+            if (inst_cream->Rd == 15) {
+                INC_PC(sizeof(rsc_inst));
+                goto DISPATCH;
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(rsc_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+
+    SADD8_INST:
+    SADD16_INST:
+    SADDSUBX_INST:
+    SSUBADDX_INST:
+    SSUB16_INST:
+    {
+        if (inst_base->cond == 0xE || CondPassed(cpu, inst_base->cond)) {
+            generic_arm_inst* const inst_cream = (generic_arm_inst*)inst_base->component;
+
+            const s16 rn_lo = (RN & 0xFFFF);
+            const s16 rn_hi = ((RN >> 16) & 0xFFFF);
+            const s16 rm_lo = (RM & 0xFFFF);
+            const s16 rm_hi = ((RM >> 16) & 0xFFFF);
+
+            s32 lo_result = 0;
+            s32 hi_result = 0;
+
+            // SADD16
+            if (inst_cream->op2 == 0x00) {
+                lo_result = (rn_lo + rm_lo);
+                hi_result = (rn_hi + rm_hi);
+            }
+            // SASX
+            else if (inst_cream->op2 == 0x01) {
+                lo_result = (rn_lo - rm_hi);
+                hi_result = (rn_hi + rm_lo);
+            }
+            // SSAX
+            else if (inst_cream->op2 == 0x02) {
+                lo_result = (rn_lo + rm_hi);
+                hi_result = (rn_hi - rm_lo);
+            }
+            // SSUB16
+            else if (inst_cream->op2 == 0x03) {
+                lo_result = (rn_lo - rm_lo);
+                hi_result = (rn_hi - rm_hi);
+            }
+
+            RD = (lo_result & 0xFFFF) | ((hi_result & 0xFFFF) << 16);
+
+            if (lo_result >= 0) {
+                cpu->Cpsr |= (1 << 16);
+                cpu->Cpsr |= (1 << 17);
+            } else {
+                cpu->Cpsr &= ~(1 << 16);
+                cpu->Cpsr &= ~(1 << 17);
+            }
+
+            if (hi_result >= 0) {
+                cpu->Cpsr |= (1 << 18);
+                cpu->Cpsr |= (1 << 19);
+            } else {
+                cpu->Cpsr &= ~(1 << 18);
+                cpu->Cpsr &= ~(1 << 19);
+            }
+        }
+
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(generic_arm_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+
+    SBC_INST:
+    {
+        sbc_inst *inst_cream = (sbc_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            lop = SHIFTER_OPERAND + !cpu->CFlag;
+            rop = RN;
+            RD = dst = rop - lop;
+            if (inst_cream->S && (inst_cream->Rd == 15)) {
+                if (CurrentModeHasSPSR) {
+                    cpu->Cpsr = cpu->Spsr_copy;
+                    switch_mode(cpu, cpu->Spsr_copy & 0x1f);
+                    LOAD_NZCVT;
+                }
+            } else if (inst_cream->S) {
+                UPDATE_NFLAG(dst);
+                UPDATE_ZFLAG(dst);
+
+                if(rop >= !cpu->CFlag)
+                    UPDATE_CFLAG_NOT_BORROW_FROM(rop - !cpu->CFlag, SHIFTER_OPERAND);
+                else
+                    UPDATE_CFLAG_NOT_BORROW_FROM(rop, !cpu->CFlag);
+
+                UPDATE_VFLAG_OVERFLOW_FROM(dst, rop, lop);
+            }
+            if (inst_cream->Rd == 15) {
+                INC_PC(sizeof(sbc_inst));
+                goto DISPATCH;
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(sbc_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+
+    SEL_INST:
+    {
+        if (inst_base->cond == 0xE || CondPassed(cpu, inst_base->cond)) {
+            generic_arm_inst* const inst_cream = (generic_arm_inst*)inst_base->component;
+
+            const u32 to = RM;
+            const u32 from = RN;
+            const u32 cpsr = cpu->Cpsr;
+
+            u32 result;
+            if (cpsr & (1 << 16))
+                result = from & 0xff;
+            else
+                result = to & 0xff;
+
+            if (cpsr & (1 << 17))
+                result |= from & 0x0000ff00;
+            else
+                result |= to & 0x0000ff00;
+
+            if (cpsr & (1 << 18))
+                result |= from & 0x00ff0000;
+            else
+                result |= to & 0x00ff0000;
+
+            if (cpsr & (1 << 19))
+                result |= from & 0xff000000;
+            else
+                result |= to & 0xff000000;
+
+            RD = result;
+        }
+
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(generic_arm_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+
+    SETEND_INST:
+    SHADD16_INST:
+    SHADD8_INST:
+    SHADDSUBX_INST:
+    SHSUB16_INST:
+    SHSUB8_INST:
+    SHSUBADDX_INST:
+    SMLA_INST:
+    {
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            smla_inst *inst_cream = (smla_inst *)inst_base->component;
+            int32_t operand1, operand2;
+            if (inst_cream->x == 0)
+                operand1 = (BIT(RM, 15)) ? (BITS(RM, 0, 15) | 0xffff0000) : BITS(RM, 0, 15);
+            else
+                operand1 = (BIT(RM, 31)) ? (BITS(RM, 16, 31) | 0xffff0000) : BITS(RM, 16, 31);
+
+            if (inst_cream->y == 0)
+                operand2 = (BIT(RS, 15)) ? (BITS(RS, 0, 15) | 0xffff0000) : BITS(RS, 0, 15);
+            else
+                operand2 = (BIT(RS, 31)) ? (BITS(RS, 16, 31) | 0xffff0000) : BITS(RS, 16, 31);
+            RD = operand1 * operand2 + RN;
+
+            // TODO: FIXME: UPDATE Q FLAGS
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(smla_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    SMLAD_INST:
+    {
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            smlad_inst *inst_cream = (smlad_inst *)inst_base->component;
+            long long int rm = cpu->Reg[inst_cream->Rm];
+            long long int rn = cpu->Reg[inst_cream->Rn];
+            long long int ra = cpu->Reg[inst_cream->Ra];
+
+            // See SMUAD
+            if(inst_cream->Ra == 15)
+                CITRA_IGNORE_EXIT(-1);
+            int operand2 = (inst_cream->m)? ROTATE_RIGHT_32(rm, 16):rm;
+            int half_rn, half_operand2;
+
+            half_rn = rn & 0xFFFF;
+            half_rn = (half_rn & 0x8000)? (0xFFFF0000|half_rn) : half_rn;
+
+            half_operand2 = operand2 & 0xFFFF;
+            half_operand2 = (half_operand2 & 0x8000)? (0xFFFF0000|half_operand2) : half_operand2;
+
+            long long int product1 = half_rn * half_operand2;
+
+            half_rn = (rn & 0xFFFF0000) >> 16;
+            half_rn = (half_rn & 0x8000)? (0xFFFF0000|half_rn) : half_rn;
+
+            half_operand2 = (operand2 & 0xFFFF0000) >> 16;
+            half_operand2 = (half_operand2 & 0x8000)? (0xFFFF0000|half_operand2) : half_operand2;
+
+            long long int product2 = half_rn * half_operand2;
+
+            long long int signed_ra = (ra & 0x80000000)? (0xFFFFFFFF00000000LL) | ra : ra;
+            long long int result = product1 + product2 + signed_ra;
+            cpu->Reg[inst_cream->Rd] = result & 0xFFFFFFFF;
+
+            // TODO: FIXME should check Signed overflow
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(umlal_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+
+    SMLAL_INST:
+    {
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            umlal_inst *inst_cream = (umlal_inst *)inst_base->component;
+            long long int rm = RM;
+            long long int rs = RS;
+            if (BIT(rm, 31)) {
+                rm |= 0xffffffff00000000LL;
+            }
+            if (BIT(rs, 31)) {
+                rs |= 0xffffffff00000000LL;
+            }
+            long long int rst = rm * rs;
+            long long int rdhi32 = RDHI;
+            long long int hilo = (rdhi32 << 32) + RDLO;
+            rst += hilo;
+            RDLO = BITS(rst,  0, 31);
+            RDHI = BITS(rst, 32, 63);
+            if (inst_cream->S) {
+                cpu->NFlag = BIT(RDHI, 31);
+                cpu->ZFlag = (RDHI == 0 && RDLO == 0);
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(umlal_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    SMLALXY_INST:
+    SMLALD_INST:
+    SMLAW_INST:
+    SMLSD_INST:
+    SMLSLD_INST:
+    SMMLA_INST:
+    SMMLS_INST:
+    SMMUL_INST:
+    SMUAD_INST:
+    SMUL_INST:
+    {
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            smul_inst *inst_cream = (smul_inst *)inst_base->component;
+            uint32_t operand1, operand2;
+            if (inst_cream->x == 0)
+                operand1 = (BIT(RM, 15)) ? (BITS(RM, 0, 15) | 0xffff0000) : BITS(RM, 0, 15);
+            else
+                operand1 = (BIT(RM, 31)) ? (BITS(RM, 16, 31) | 0xffff0000) : BITS(RM, 16, 31);
+
+            if (inst_cream->y == 0)
+                operand2 = (BIT(RS, 15)) ? (BITS(RS, 0, 15) | 0xffff0000) : BITS(RS, 0, 15);
+            else
+                operand2 = (BIT(RS, 31)) ? (BITS(RS, 16, 31) | 0xffff0000) : BITS(RS, 16, 31);
+            RD = operand1 * operand2;
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(smul_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    SMULL_INST:
+    {
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            umull_inst *inst_cream = (umull_inst *)inst_base->component;
+            int64_t rm = RM;
+            int64_t rs = RS;
+            if (BIT(rm, 31)) {
+                rm |= 0xffffffff00000000LL;
+            }
+            if (BIT(rs, 31)) {
+                rs |= 0xffffffff00000000LL;
+            }
+            int64_t rst = rm * rs;
+            RDHI = BITS(rst, 32, 63);
+            RDLO = BITS(rst,  0, 31);
+
+            if (inst_cream->S) {
+                cpu->NFlag = BIT(RDHI, 31);
+                cpu->ZFlag = (RDHI == 0 && RDLO == 0);
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(umull_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+
+    SMULW_INST:
+    {
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            smlad_inst *inst_cream = (smlad_inst *)inst_base->component;
+            int64_t rm = RM;
+            int64_t rn = RN;
+            if (inst_cream->m)
+                rm = BITS(rm, 16, 31);
+            else
+                rm = BITS(rm, 0, 15);
+            int64_t rst = rm * rn;
+            RD = BITS(rst, 16, 47);
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(smlad_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+
+    SMUSD_INST:
+    SRS_INST:
+    SSAT_INST:
+    SSAT16_INST:
+    SSUB8_INST:
+    STC_INST:
+    {
+        // Instruction not implemented
+        //LOG_CRITICAL(Core_ARM11, "unimplemented instruction");
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(stc_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    STM_INST:
+    {
+        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+        unsigned int inst = inst_cream->inst;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            int i;
+            unsigned int Rn = BITS(inst, 16, 19);
+            unsigned int old_RN = cpu->Reg[Rn];
+
+            fault = inst_cream->get_addr(cpu, inst_cream->inst, addr, phys_addr, 0);
+            if (fault) goto MMU_EXCEPTION;
+            if (BIT(inst_cream->inst, 22) == 1) {
+                for (i = 0; i < 13; i++) {
+                    if(BIT(inst_cream->inst, i)){
+                        fault = check_address_validity(cpu, addr, &phys_addr, 0);
+                        if (fault) goto MMU_EXCEPTION;
+                        fault = interpreter_write_memory(addr, phys_addr, cpu->Reg[i], 32);
+                        if (fault) goto MMU_EXCEPTION;
+                        addr += 4;
+                        phys_addr += 4;
+                    }
+                }
+                if (BIT(inst_cream->inst, 13)) {
+                    if (cpu->Mode == USER32MODE) {
+                        fault = check_address_validity(cpu, addr, &phys_addr, 0);
+                        if (fault) goto MMU_EXCEPTION;
+                        fault = interpreter_write_memory(addr, phys_addr, cpu->Reg[i], 32);
+                        if (fault) goto MMU_EXCEPTION;
+                        addr += 4;
+                        phys_addr += 4;
+                    } else {
+                        fault = interpreter_write_memory(addr, phys_addr, cpu->Reg_usr[0], 32);
+                        if (fault) goto MMU_EXCEPTION;
+                        addr += 4;
+                        phys_addr += 4;
+                    }
+                }
+                if (BIT(inst_cream->inst, 14)) {
+                    if (cpu->Mode == USER32MODE) {
+                        fault = check_address_validity(cpu, addr, &phys_addr, 0);
+                        if (fault) goto MMU_EXCEPTION;
+                        fault = interpreter_write_memory(addr, phys_addr, cpu->Reg[i], 32);
+                        if (fault) goto MMU_EXCEPTION;
+                        addr += 4;
+                        phys_addr += 4;
+                    } else {
+                        fault = check_address_validity(cpu, addr, &phys_addr, 0);
+                        if (fault) goto MMU_EXCEPTION;
+                        fault = interpreter_write_memory(addr, phys_addr, cpu->Reg_usr[1], 32);
+                        if (fault) goto MMU_EXCEPTION;
+                        addr += 4;
+                        phys_addr += 4;
+                    }
+                }
+                if (BIT(inst_cream->inst, 15)) {
+                    fault = check_address_validity(cpu, addr, &phys_addr, 0);
+                    if (fault) goto MMU_EXCEPTION;
+                    fault = interpreter_write_memory(addr, phys_addr, cpu->Reg[i] + 8, 32);
+                    if (fault) goto MMU_EXCEPTION;
+                }
+            } else {
+                for( i = 0; i < 15; i ++ ) {
+                    if(BIT(inst_cream->inst, i)) {
+                        fault = check_address_validity(cpu, addr, &phys_addr, 0);
+                        if (fault) goto MMU_EXCEPTION;
+                        if(i == Rn)
+                            fault = interpreter_write_memory(addr, phys_addr, old_RN, 32);
+                        else
+                            fault = interpreter_write_memory(addr, phys_addr, cpu->Reg[i], 32);
+                        if (fault) goto MMU_EXCEPTION;
+                        addr += 4;
+                        phys_addr += 4;
+                    }
+                }
+
+                // Check PC reg
+                if(BIT(inst_cream->inst, i)) {
+                    fault = check_address_validity(cpu, addr, &phys_addr, 0);
+                    if (fault) goto MMU_EXCEPTION;
+                    fault = interpreter_write_memory(addr, phys_addr, cpu->Reg[i] + 8, 32);
+                    if (fault) goto MMU_EXCEPTION;
+                }
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(ldst_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    SXTB_INST:
+    {
+        sxtb_inst *inst_cream = (sxtb_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            if (inst_cream->Rm == 15) {
+                LOG_ERROR(Core_ARM11, "invalid operand for SXTB");
+                CITRA_IGNORE_EXIT(-1);
+            }
+            unsigned int operand2 = ROTATE_RIGHT_32(RM, 8 * inst_cream->rotate);
+            if (BIT(operand2, 7)) {
+                operand2 |= 0xffffff00;
+            } else
+                operand2 &= 0xff;
+            RD = operand2;
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(sxtb_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    STR_INST:
+    {
+        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            fault = inst_cream->get_addr(cpu, inst_cream->inst, addr, phys_addr, 0);
+            if (fault) goto MMU_EXCEPTION;
+            unsigned int value = cpu->Reg[BITS(inst_cream->inst, 12, 15)];
+            fault = interpreter_write_memory(addr, phys_addr, value, 32);
+            if (fault) goto MMU_EXCEPTION;
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(ldst_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    UXTB_INST:
+    {
+        uxtb_inst *inst_cream = (uxtb_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            unsigned int operand2 = ROTATE_RIGHT_32(RM, 8 * inst_cream->rotate) 
+                        & 0xff;
+            RD = operand2;
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(uxtb_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    UXTAB_INST:
+    {
+        uxtab_inst *inst_cream = (uxtab_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            unsigned int operand2 = ROTATE_RIGHT_32(RM, 8 * inst_cream->rotate) 
+                        & 0xff;
+            RD = RN + operand2;
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(uxtab_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    STRB_INST:
+    {
+        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            fault = inst_cream->get_addr(cpu, inst_cream->inst, addr, phys_addr, 0);
+            if (fault) goto MMU_EXCEPTION;
+            unsigned int value = cpu->Reg[BITS(inst_cream->inst, 12, 15)] & 0xff;
+            fault = interpreter_write_memory(addr, phys_addr, value, 8);
+            if (fault) goto MMU_EXCEPTION;
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(ldst_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    STRBT_INST:
+    {
+        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            fault = inst_cream->get_addr(cpu, inst_cream->inst, addr, phys_addr, 0);
+            if (fault) goto MMU_EXCEPTION;
+            unsigned int value = cpu->Reg[BITS(inst_cream->inst, 12, 15)] & 0xff;
+            fault = interpreter_write_memory(addr, phys_addr, value, 8);
+            if (fault) goto MMU_EXCEPTION;
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(ldst_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    STRD_INST:
+    {
+        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            fault = inst_cream->get_addr(cpu, inst_cream->inst, addr, phys_addr, 0);
+            if (fault) goto MMU_EXCEPTION;
+            uint32_t rear_phys_addr;
+            fault = check_address_validity(cpu, addr + 4, &rear_phys_addr, 0);
                         if (fault){
-                                LOG_ERROR(Core_ARM11, "mmu fault , should rollback the above get_addr\n");
-                                CITRA_IGNORE_EXIT(-1);
-                                goto MMU_EXCEPTION;
-                        }
-
-                        //fault = inst_cream->get_addr(cpu, inst_cream->inst, addr + 4, phys_addr + 4, 0);
-                        //if (fault) goto MMU_EXCEPTION;
-
-                        unsigned int value = cpu->Reg[BITS(inst_cream->inst, 12, 15)];
-                        //bus_write(32, addr, value);
-                        fault = interpreter_write_memory(addr, phys_addr, value, 32);
-                        if (fault) goto MMU_EXCEPTION;
-                        value = cpu->Reg[BITS(inst_cream->inst, 12, 15) + 1];
-                        //bus_write(32, addr, value);
-                        fault = interpreter_write_memory(addr + 4, rear_phys_addr, value, 32);
-                        if (fault) goto MMU_EXCEPTION;
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(ldst_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        STREX_INST:
-        {
-                INC_ICOUNTER;
-                ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        addr = cpu->Reg[BITS(inst_cream->inst, 16, 19)];
-                        unsigned int value = cpu->Reg[BITS(inst_cream->inst, 0, 3)];
-                        fault = check_address_validity(cpu, addr, &phys_addr, 0);
-                        if (fault) goto MMU_EXCEPTION;
-
-                        int dest_reg = BITS(inst_cream->inst, 12, 15);
-                        if((exclusive_detect(cpu, phys_addr) == 0) && (cpu->exclusive_state == 1)){
-                                remove_exclusive(cpu, phys_addr);
-                                cpu->Reg[dest_reg] = 0;
-                                cpu->exclusive_state = 0;
-                                
-                                //                      bus_write(32, addr, value);
-                                fault = interpreter_write_memory(addr, phys_addr, value, 32);
-                                if (fault) goto MMU_EXCEPTION;
-                        }
-                        else{
-                                /* Failed to write due to mutex access */
-                                cpu->Reg[dest_reg] = 1;
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(ldst_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        STREXB_INST:
-        {
-                INC_ICOUNTER;
-                ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        addr = cpu->Reg[BITS(inst_cream->inst, 16, 19)];
-                        unsigned int value = cpu->Reg[BITS(inst_cream->inst, 0, 3)] & 0xff;
-                        fault = check_address_validity(cpu, addr, &phys_addr, 0);
-                        if (fault) goto MMU_EXCEPTION;
-                        //bus_write(8, addr, value);
-                        int dest_reg = BITS(inst_cream->inst, 12, 15);
-                        if((exclusive_detect(cpu, phys_addr) == 0) && (cpu->exclusive_state == 1)){
-                                remove_exclusive(cpu, phys_addr);
-                                cpu->Reg[dest_reg] = 0;
-                                cpu->exclusive_state = 0;
-                                fault = interpreter_write_memory(addr, phys_addr, value, 8);
-                                if (fault) goto MMU_EXCEPTION;
-
-                        }
-                        else{
-                                cpu->Reg[dest_reg] = 1;
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(ldst_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        STRH_INST:
-        {
-                INC_ICOUNTER;
-                ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        fault = inst_cream->get_addr(cpu, inst_cream->inst, addr, phys_addr, 0);
-                        if (fault) goto MMU_EXCEPTION;
-                        unsigned int value = cpu->Reg[BITS(inst_cream->inst, 12, 15)] & 0xffff;
-                        //bus_write(16, addr, value);
-                        fault = interpreter_write_memory(addr, phys_addr, value, 16);
-                        if (fault) goto MMU_EXCEPTION;
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                //if (BITS(inst_cream->inst, 12, 15) == 15)
-                //      goto DISPATCH;
-                INC_PC(sizeof(ldst_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        STRT_INST:
-        {
-                INC_ICOUNTER;
-                ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        fault = inst_cream->get_addr(cpu, inst_cream->inst, addr, phys_addr, 0);
-                        if (fault) goto MMU_EXCEPTION;
-                        unsigned int value = cpu->Reg[BITS(inst_cream->inst, 12, 15)];
-                        //bus_write(16, addr, value);
-                        fault = interpreter_write_memory(addr, phys_addr, value, 32);
-                        if (fault) goto MMU_EXCEPTION;
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(ldst_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        SUB_INST:
-        {
-                INC_ICOUNTER;
-                sub_inst *inst_cream = (sub_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        lop = RN;
-                        if (inst_cream->Rn == 15) {
-                                lop += 8;
-                        }
-                        rop = SHIFTER_OPERAND;
-                        RD = dst = lop - rop;
-                        if (inst_cream->S && (inst_cream->Rd == 15)) {
-                                /* cpsr = spsr */
-                                if (CurrentModeHasSPSR) {
-                                        cpu->Cpsr = cpu->Spsr_copy;
-                                        switch_mode(cpu, cpu->Spsr_copy & 0x1f);
-                                        LOAD_NZCVT;
-                                }
-                        } else if (inst_cream->S) {
-                                UPDATE_NFLAG(dst);
-                                UPDATE_ZFLAG(dst);
-//                              UPDATE_CFLAG(dst, lop, rop);
-                                UPDATE_CFLAG_NOT_BORROW_FROM(lop, rop);
-        //                      UPDATE_VFLAG((int)dst, (int)lop, (int)rop);
-                                UPDATE_VFLAG_OVERFLOW_FROM(dst, lop, rop);
-                        }
-                        if (inst_cream->Rd == 15) {
-                                INC_PC(sizeof(sub_inst));
-                                goto DISPATCH;
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(sub_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        SWI_INST:
-        {
-                INC_ICOUNTER;
-                swi_inst *inst_cream = (swi_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        if (true){ //if (core->is_user_mode) { --> Citra only emulates user mode
-                                //arm_dyncom_SWI(cpu, inst_cream->num);
-                                HLE::CallSVC(Memory::Read32(cpu->Reg[15]));
-                        } else {
-                                cpu->syscallSig = 1;
-                                goto END;
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(swi_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        SWP_INST:
-        {
-                INC_ICOUNTER;
-                swp_inst *inst_cream = (swp_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        addr = RN;
-                        fault = check_address_validity(cpu, addr, &phys_addr, 1);
-                        if (fault) goto MMU_EXCEPTION;
-                        unsigned int value;
-                        fault = interpreter_read_memory(addr, phys_addr, value, 32);
-                        if (fault) goto MMU_EXCEPTION;
-                        fault = interpreter_write_memory(addr, phys_addr, RM, 32);
-                        if (fault) goto MMU_EXCEPTION;
-
-                        /* ROR(data, 8*UInt(address<1:0>)); */
-                        assert((phys_addr & 0x3) == 0);
-                        RD = value;
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(swp_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        SWPB_INST:
-        {
-                INC_ICOUNTER;
-                swp_inst *inst_cream = (swp_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        addr = RN;
-                        fault = check_address_validity(cpu, addr, &phys_addr, 1);
-                        if (fault) goto MMU_EXCEPTION;
-                        unsigned int value;
-                        fault = interpreter_read_memory(addr, phys_addr, value, 8);
-                        if (fault) goto MMU_EXCEPTION;
-                        fault = interpreter_write_memory(addr, phys_addr, (RM & 0xFF), 8);
-                        if (fault) goto MMU_EXCEPTION;
-
-                        /* FIXME */
-                        #if 0
-                        if Shared(address) then
-                        /* ARMv6 */
-                        physical_address = TLB(address)
-                        ClearExclusiveByAddress(physical_address,processor_id,1)
-                        /* See Summary of operation on page A2-49 */
-                        #endif
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(swp_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        SXTAB_INST:
-        {
-                INC_ICOUNTER;
-                sxtab_inst *inst_cream = (sxtab_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        /* R15 should be check */
-                        if(inst_cream->Rn == 15 || inst_cream->Rm == 15 || inst_cream->Rd ==15){
-                                CITRA_IGNORE_EXIT(-1);
-                        }
-                        unsigned int operand2 = ROTATE_RIGHT_32(RM, 8 * inst_cream->rotate) 
-                                                & 0xff;
-                        /* sign extend for byte */
-                        operand2 = (0x80 & operand2)? (0xFFFFFF00 | operand2):operand2;
-                        RD = RN + operand2;
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(uxtab_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        SXTAB16_INST:
-        SXTAH_INST:
-        {
-                INC_ICOUNTER;
-                sxtah_inst *inst_cream = (sxtah_inst *)inst_base->component;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        /* R15 should be check */
-                        if(inst_cream->Rn == 15 || inst_cream->Rm == 15 || inst_cream->Rd ==15){
-                                CITRA_IGNORE_EXIT(-1);
-                        }
-                        unsigned int operand2 = ROTATE_RIGHT_32(RM, 8 * inst_cream->rotate) & 0xffff;
-                        /* sign extend for half */
-                        operand2 = (0x8000 & operand2)? (0xFFFF0000 | operand2):operand2;
-                        RD = RN + operand2;
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(sxtah_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        SXTB16_INST:
-        TEQ_INST:
-        {
-                INC_ICOUNTER;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        teq_inst *inst_cream = (teq_inst *)inst_base->component;
-                        lop = RN;
-                        if (inst_cream->Rn == 15)
-                                lop += GET_INST_SIZE(cpu) * 2;
-
-                        rop = SHIFTER_OPERAND;
-                        dst = lop ^ rop;
-
-                        UPDATE_NFLAG(dst);
-                        UPDATE_ZFLAG(dst);
-                        UPDATE_CFLAG_WITH_SC;
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(teq_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        TST_INST:
-        {
-                INC_ICOUNTER;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        tst_inst *inst_cream = (tst_inst *)inst_base->component;
-                        lop = RN;
-                        if (inst_cream->Rn == 15)
-                                lop += GET_INST_SIZE(cpu) * 2;
-                        rop = SHIFTER_OPERAND;
-                        dst = lop & rop;
-
-                        UPDATE_NFLAG(dst);
-                        UPDATE_ZFLAG(dst);
-                        UPDATE_CFLAG_WITH_SC;
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(tst_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        UADD16_INST:
-        UADD8_INST:
-        UADDSUBX_INST:
-        UHADD16_INST:
-        UHADD8_INST:
-        UHADDSUBX_INST:
-        UHSUB16_INST:
-        UHSUB8_INST:
-        UHSUBADDX_INST:
-        UMAAL_INST:
-        UMLAL_INST:
-        {
-                INC_ICOUNTER;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        umlal_inst *inst_cream = (umlal_inst *)inst_base->component;
-                        unsigned long long int rm = RM;
-                        unsigned long long int rs = RS;
-                        unsigned long long int rst = rm * rs;
-                        unsigned long long int add = ((unsigned long long) RDHI)<<32;
-                        add += RDLO;
-                        //DEBUG_LOG(ARM11, "rm[%llx] * rs[%llx] = rst[%llx] | add[%llx]\n", RM, RS, rst, add);
-                        rst += add;
-                        RDLO = BITS(rst,  0, 31);
-                        RDHI = BITS(rst, 32, 63);
-                        
-                        if (inst_cream->S)
-                        {
-                                cpu->NFlag = BIT(RDHI, 31);
-                                cpu->ZFlag = (RDHI == 0 && RDLO == 0);
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(umlal_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        UMULL_INST:
-        {
-                INC_ICOUNTER;
-                if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
-                        umull_inst *inst_cream = (umull_inst *)inst_base->component;
-                        unsigned long long int rm = RM;
-                        unsigned long long int rs = RS;
-                        unsigned long long int rst = rm * rs;
-//                      DEBUG_LOG(ARM11, "rm : [%llx] rs : [%llx] rst [%llx]\n", RM, RS, rst);
-                        RDHI = BITS(rst, 32, 63);
-                        RDLO = BITS(rst,  0, 31);
-
-                        if (inst_cream->S) {
-                                cpu->NFlag = BIT(RDHI, 31);
-                                cpu->ZFlag = (RDHI == 0 && RDLO == 0);
-                        }
-                }
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(umull_inst));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-        }
-        B_2_THUMB:
-        {
-                INC_ICOUNTER;
-                b_2_thumb *inst_cream = (b_2_thumb *)inst_base->component;
-                cpu->Reg[15] = cpu->Reg[15] + 4 + inst_cream->imm;
-                //DEBUG_LOG(ARM11, " BL_1_THUMB: imm=0x%x, r14=0x%x, r15=0x%x\n", inst_cream->imm, cpu->Reg[14], cpu->Reg[15]);
-                INC_PC(sizeof(b_2_thumb));
-                goto DISPATCH;
-        }
-        B_COND_THUMB:
-        {
-                INC_ICOUNTER;
-                b_cond_thumb *inst_cream = (b_cond_thumb *)inst_base->component;
-                if(CondPassed(cpu, inst_cream->cond))
-                        cpu->Reg[15] = cpu->Reg[15] + 4 + inst_cream->imm;
-                else
-                        cpu->Reg[15] += 2;
-                //DEBUG_LOG(ARM11, " B_COND_THUMB: imm=0x%x, r15=0x%x\n", inst_cream->imm, cpu->Reg[15]);
-                INC_PC(sizeof(b_cond_thumb));
-                goto DISPATCH;
-        }
-        BL_1_THUMB:
-        {
-                INC_ICOUNTER;
-                bl_1_thumb *inst_cream = (bl_1_thumb *)inst_base->component;
-                cpu->Reg[14] = cpu->Reg[15] + 4 + inst_cream->imm;
-                //cpu->Reg[15] += 2;
-                //DEBUG_LOG(ARM11, " BL_1_THUMB: imm=0x%x, r14=0x%x, r15=0x%x\n", inst_cream->imm, cpu->Reg[14], cpu->Reg[15]);
-
-                cpu->Reg[15] += GET_INST_SIZE(cpu);
-                INC_PC(sizeof(bl_1_thumb));
-                FETCH_INST;
-                GOTO_NEXT_INST;
-
-        }
-        BL_2_THUMB:
-        {
-                INC_ICOUNTER;
-                bl_2_thumb *inst_cream = (bl_2_thumb *)inst_base->component;
-                int tmp = ((cpu->Reg[15] + 2) | 1);
-                cpu->Reg[15] =
-                        (cpu->Reg[14] + inst_cream->imm);
-                cpu->Reg[14] = tmp;
-                //DEBUG_LOG(ARM11, " BL_2_THUMB: imm=0x%x, r14=0x%x, r15=0x%x\n", inst_cream->imm, cpu->Reg[14], cpu->Reg[15]);
-                INC_PC(sizeof(bl_2_thumb));
-                goto DISPATCH;
-        }
-        BLX_1_THUMB:
-        {       
-                /* BLX 1 for armv5t and above */
-                INC_ICOUNTER;
-                uint32 tmp = cpu->Reg[15];
-                blx_1_thumb *inst_cream = (blx_1_thumb *)inst_base->component;
-                cpu->Reg[15] = (cpu->Reg[14] + inst_cream->imm) & 0xFFFFFFFC;
-                //DEBUG_LOG(ARM11, "In BLX_1_THUMB, BLX(1),imm=0x%x,r14=0x%x, instr=0x%x\n", inst_cream->imm, cpu->Reg[14], inst_cream->instr);
-                cpu->Reg[14] = ((tmp + 2) | 1);
-                //(state->Reg[14] + ((tinstr & 0x07FF) << 1)) & 0xFFFFFFFC;
-                /* switch to arm state from thumb state */
-                cpu->TFlag = 0;
-                //DEBUG_LOG(ARM11, "In BLX_1_THUMB, BLX(1),imm=0x%x,r14=0x%x, r15=0x%x, \n", inst_cream->imm, cpu->Reg[14], cpu->Reg[15]);
-                INC_PC(sizeof(blx_1_thumb));
-                goto DISPATCH;
-        }
-
-        UQADD16_INST:
-        UQADD8_INST:
-        UQADDSUBX_INST:
-        UQSUB16_INST:
-        UQSUB8_INST:
-        UQSUBADDX_INST:
-        USAD8_INST:
-        USADA8_INST:
-        USAT_INST:
-        USAT16_INST:
-        USUB16_INST:
-        USUB8_INST:
-        USUBADDX_INST:
-        UXTAB16_INST:
-        UXTB16_INST:
-        #define VFP_INTERPRETER_IMPL
-        #include "core/arm/skyeye_common/vfp/vfpinstr.cpp"
-        #undef VFP_INTERPRETER_IMPL
-        MMU_EXCEPTION:
-        {
-                SAVE_NZCVT;
-                cpu->abortSig = true;
-                cpu->Aborted = ARMul_DataAbortV;
-                cpu->AbortAddr = addr;
-                cpu->CP15[CP15(CP15_FAULT_STATUS)] = fault & 0xff;
-                cpu->CP15[CP15(CP15_FAULT_ADDRESS)] = addr;
-                cpu->NumInstrsToExecute = 0;
-                return num_instrs;
-        }
-        END:
-        {
-                SAVE_NZCVT;
-                cpu->NumInstrsToExecute = 0;
-                return num_instrs;
-        }
-        INIT_INST_LENGTH:
-        {
-#if 0
-        DEBUG_LOG(ARM11, "InstLabel:%d\n", sizeof(InstLabel));
-        for (int i = 0; i < (sizeof(InstLabel) / sizeof(void *)); i ++)
-                DEBUG_LOG(ARM11, "[%llx]\n", InstLabel[i]);
-        DEBUG_LOG(ARM11, "InstLabel:%d\n", sizeof(InstLabel));
-#endif
+                LOG_ERROR(Core_ARM11, "MMU fault , should rollback the above get_addr");
+                CITRA_IGNORE_EXIT(-1);
+                goto MMU_EXCEPTION;
+            }
+
+            unsigned int value = cpu->Reg[BITS(inst_cream->inst, 12, 15)];
+            fault = interpreter_write_memory(addr, phys_addr, value, 32);
+            if (fault) goto MMU_EXCEPTION;
+            value = cpu->Reg[BITS(inst_cream->inst, 12, 15) + 1];
+            fault = interpreter_write_memory(addr + 4, rear_phys_addr, value, 32);
+            if (fault) goto MMU_EXCEPTION;
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(ldst_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    STREX_INST:
+    {
+        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            addr = cpu->Reg[BITS(inst_cream->inst, 16, 19)];
+            unsigned int value = cpu->Reg[BITS(inst_cream->inst, 0, 3)];
+            fault = check_address_validity(cpu, addr, &phys_addr, 0);
+            if (fault) goto MMU_EXCEPTION;
+
+            int dest_reg = BITS(inst_cream->inst, 12, 15);
+            if((exclusive_detect(cpu, phys_addr) == 0) && (cpu->exclusive_state == 1)){
+                remove_exclusive(cpu, phys_addr);
+                cpu->Reg[dest_reg] = 0;
+                cpu->exclusive_state = 0;
+
+                fault = interpreter_write_memory(addr, phys_addr, value, 32);
+                if (fault) goto MMU_EXCEPTION;
+            } else {
+                // Failed to write due to mutex access
+                cpu->Reg[dest_reg] = 1;
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(ldst_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    STREXB_INST:
+    {
+        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            addr = cpu->Reg[BITS(inst_cream->inst, 16, 19)];
+            unsigned int value = cpu->Reg[BITS(inst_cream->inst, 0, 3)] & 0xff;
+            fault = check_address_validity(cpu, addr, &phys_addr, 0);
+            if (fault) goto MMU_EXCEPTION;
+            int dest_reg = BITS(inst_cream->inst, 12, 15);
+            if((exclusive_detect(cpu, phys_addr) == 0) && (cpu->exclusive_state == 1)){
+                remove_exclusive(cpu, phys_addr);
+                cpu->Reg[dest_reg] = 0;
+                cpu->exclusive_state = 0;
+                fault = interpreter_write_memory(addr, phys_addr, value, 8);
+                if (fault) goto MMU_EXCEPTION;
+            } else {
+                cpu->Reg[dest_reg] = 1;
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(ldst_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    STRH_INST:
+    {
+        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            fault = inst_cream->get_addr(cpu, inst_cream->inst, addr, phys_addr, 0);
+            if (fault) goto MMU_EXCEPTION;
+            unsigned int value = cpu->Reg[BITS(inst_cream->inst, 12, 15)] & 0xffff;
+            fault = interpreter_write_memory(addr, phys_addr, value, 16);
+            if (fault) goto MMU_EXCEPTION;
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(ldst_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    STRT_INST:
+    {
+        ldst_inst *inst_cream = (ldst_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            fault = inst_cream->get_addr(cpu, inst_cream->inst, addr, phys_addr, 0);
+            if (fault) goto MMU_EXCEPTION;
+            unsigned int value = cpu->Reg[BITS(inst_cream->inst, 12, 15)];
+            fault = interpreter_write_memory(addr, phys_addr, value, 32);
+            if (fault) goto MMU_EXCEPTION;
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(ldst_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    SUB_INST:
+    {
+        sub_inst *inst_cream = (sub_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            lop = RN;
+            if (inst_cream->Rn == 15) {
+                lop += 8;
+            }
+            rop = SHIFTER_OPERAND;
+            RD = dst = lop - rop;
+            if (inst_cream->S && (inst_cream->Rd == 15)) {
+                if (CurrentModeHasSPSR) {
+                    cpu->Cpsr = cpu->Spsr_copy;
+                    switch_mode(cpu, cpu->Spsr_copy & 0x1f);
+                    LOAD_NZCVT;
+                }
+            } else if (inst_cream->S) {
+                UPDATE_NFLAG(dst);
+                UPDATE_ZFLAG(dst);
+                UPDATE_CFLAG_NOT_BORROW_FROM(lop, rop);
+                UPDATE_VFLAG_OVERFLOW_FROM(dst, lop, rop);
+            }
+            if (inst_cream->Rd == 15) {
+                INC_PC(sizeof(sub_inst));
+                goto DISPATCH;
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(sub_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    SWI_INST:
+    {
+        swi_inst *inst_cream = (swi_inst *)inst_base->component;
+
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond))
+            HLE::CallSVC(Memory::Read32(cpu->Reg[15]));
+
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(swi_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    SWP_INST:
+    {
+        swp_inst *inst_cream = (swp_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            addr = RN;
+            fault = check_address_validity(cpu, addr, &phys_addr, 1);
+            if (fault) goto MMU_EXCEPTION;
+            unsigned int value;
+            fault = interpreter_read_memory(addr, phys_addr, value, 32);
+            if (fault) goto MMU_EXCEPTION;
+            fault = interpreter_write_memory(addr, phys_addr, RM, 32);
+            if (fault) goto MMU_EXCEPTION;
+
+            assert((phys_addr & 0x3) == 0);
+            RD = value;
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(swp_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    SWPB_INST:
+    {
+        swp_inst *inst_cream = (swp_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            addr = RN;
+            fault = check_address_validity(cpu, addr, &phys_addr, 1);
+            if (fault) goto MMU_EXCEPTION;
+            unsigned int value;
+            fault = interpreter_read_memory(addr, phys_addr, value, 8);
+            if (fault) goto MMU_EXCEPTION;
+            fault = interpreter_write_memory(addr, phys_addr, (RM & 0xFF), 8);
+            if (fault) goto MMU_EXCEPTION;
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(swp_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    SXTAB_INST:
+    {
+        sxtab_inst *inst_cream = (sxtab_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            // R15 should be check
+            if(inst_cream->Rn == 15 || inst_cream->Rm == 15 || inst_cream->Rd ==15){
+                CITRA_IGNORE_EXIT(-1);
+            }
+            unsigned int operand2 = ROTATE_RIGHT_32(RM, 8 * inst_cream->rotate) & 0xff;
+
+            // Sign extend for byte
+            operand2 = (0x80 & operand2)? (0xFFFFFF00 | operand2):operand2;
+            RD = RN + operand2;
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(uxtab_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    SXTAB16_INST:
+    SXTAH_INST:
+    {
+        sxtah_inst *inst_cream = (sxtah_inst *)inst_base->component;
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            // R15 should be check
+            if(inst_cream->Rn == 15 || inst_cream->Rm == 15 || inst_cream->Rd ==15) {
+                CITRA_IGNORE_EXIT(-1);
+            }
+            unsigned int operand2 = ROTATE_RIGHT_32(RM, 8 * inst_cream->rotate) & 0xffff;
+            // Sign extend for half
+            operand2 = (0x8000 & operand2) ? (0xFFFF0000 | operand2) : operand2;
+            RD = RN + operand2;
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(sxtah_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    SXTB16_INST:
+    TEQ_INST:
+    {
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            teq_inst *inst_cream = (teq_inst *)inst_base->component;
+            lop = RN;
+
+            if (inst_cream->Rn == 15)
+                lop += GET_INST_SIZE(cpu) * 2;
+
+            rop = SHIFTER_OPERAND;
+            dst = lop ^ rop;
+
+            UPDATE_NFLAG(dst);
+            UPDATE_ZFLAG(dst);
+            UPDATE_CFLAG_WITH_SC;
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(teq_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    TST_INST:
+    {
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            tst_inst *inst_cream = (tst_inst *)inst_base->component;
+            lop = RN;
+
+            if (inst_cream->Rn == 15)
+                lop += GET_INST_SIZE(cpu) * 2;
+
+            rop = SHIFTER_OPERAND;
+            dst = lop & rop;
+
+            UPDATE_NFLAG(dst);
+            UPDATE_ZFLAG(dst);
+            UPDATE_CFLAG_WITH_SC;
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(tst_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    UADD8_INST:
+    UADD16_INST:
+    UADDSUBX_INST:
+
+    UHADD8_INST:
+    UHADD16_INST:
+    UHADDSUBX_INST:
+    UHSUBADDX_INST:
+    UHSUB8_INST:
+    UHSUB16_INST:
+    {
+        if (inst_base->cond == 0xE || CondPassed(cpu, inst_base->cond)) {
+            generic_arm_inst* const inst_cream = (generic_arm_inst*)inst_base->component;
+            const u32 rm_val = RM;
+            const u32 rn_val = RN;
+            const u8 op2 = inst_cream->op2;
+
+            if (op2 == 0x00 || op2 == 0x01 || op2 == 0x02 || op2 == 0x03)
+            {
+                u32 lo_val = 0;
+                u32 hi_val = 0;
+                
+                // UHADD16
+                if (op2 == 0x00) {
+                    lo_val = (rn_val & 0xFFFF) + (rm_val & 0xFFFF);
+                    hi_val = ((rn_val >> 16) & 0xFFFF) + ((rm_val >> 16) & 0xFFFF);
+                }
+                // UHASX
+                else if (op2 == 0x01) {
+                    lo_val = (rn_val & 0xFFFF) - ((rm_val >> 16) & 0xFFFF);
+                    hi_val = ((rn_val >> 16) & 0xFFFF) + (rm_val & 0xFFFF);
+                }
+                // UHSAX
+                else if (op2 == 0x02) {
+                    lo_val = (rn_val & 0xFFFF) + ((rm_val >> 16) & 0xFFFF);
+                    hi_val = ((rn_val >> 16) & 0xFFFF) - (rm_val & 0xFFFF);
+                }
+                // UHSUB16
+                else if (op2 == 0x03) {
+                    lo_val = (rn_val & 0xFFFF) - (rm_val & 0xFFFF);
+                    hi_val = ((rn_val >> 16) & 0xFFFF) - ((rm_val >> 16) & 0xFFFF);
+                }
+
+                lo_val >>= 1;
+                hi_val >>= 1;
+
+                RD = (lo_val & 0xFFFF) | ((hi_val & 0xFFFF) << 16);
+            }
+            else if (op2 == 0x04 || op2 == 0x07) {
+                u32 sum1;
+                u32 sum2;
+                u32 sum3;
+                u32 sum4;
+
+                // UHADD8
+                if (op2 == 0x04) {
+                    sum1 = (rn_val & 0xFF) + (rm_val & 0xFF);
+                    sum2 = ((rn_val >> 8) & 0xFF) + ((rm_val >> 8) & 0xFF);
+                    sum3 = ((rn_val >> 16) & 0xFF) + ((rm_val >> 16) & 0xFF);
+                    sum4 = ((rn_val >> 24) & 0xFF) + ((rm_val >> 24) & 0xFF);
+                }
+                // UHSUB8
+                else {
+                    sum1 = (rn_val & 0xFF) - (rm_val & 0xFF);
+                    sum2 = ((rn_val >> 8) & 0xFF) - ((rm_val >> 8) & 0xFF);
+                    sum3 = ((rn_val >> 16) & 0xFF) - ((rm_val >> 16) & 0xFF);
+                    sum4 = ((rn_val >> 24) & 0xFF) - ((rm_val >> 24) & 0xFF);
+                }
+
+                sum1 >>= 1;
+                sum2 >>= 1;
+                sum3 >>= 1;
+                sum4 >>= 1;
+
+                RD = (sum1 & 0xFF) | ((sum2 & 0xFF) << 8) | ((sum3 & 0xFF) << 16) | ((sum4 & 0xFF) << 24);
+            }
+        }
+
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(generic_arm_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+
+    UMAAL_INST:
+    {
+        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:
+    {
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            umlal_inst *inst_cream = (umlal_inst *)inst_base->component;
+            unsigned long long int rm = RM;
+            unsigned long long int rs = RS;
+            unsigned long long int rst = rm * rs;
+            unsigned long long int add = ((unsigned long long) RDHI)<<32;
+            add += RDLO;
+            rst += add;
+            RDLO = BITS(rst,  0, 31);
+            RDHI = BITS(rst, 32, 63);
+
+            if (inst_cream->S) {
+                cpu->NFlag = BIT(RDHI, 31);
+                cpu->ZFlag = (RDHI == 0 && RDLO == 0);
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(umlal_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    UMULL_INST:
+    {
+        if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
+            umull_inst *inst_cream = (umull_inst *)inst_base->component;
+            unsigned long long int rm = RM;
+            unsigned long long int rs = RS;
+            unsigned long long int rst = rm * rs;
+            RDHI = BITS(rst, 32, 63);
+            RDLO = BITS(rst,  0, 31);
+
+            if (inst_cream->S) {
+                cpu->NFlag = BIT(RDHI, 31);
+                cpu->ZFlag = (RDHI == 0 && RDLO == 0);
+            }
+        }
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(umull_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    B_2_THUMB:
+    {
+        b_2_thumb *inst_cream = (b_2_thumb *)inst_base->component;
+        cpu->Reg[15] = cpu->Reg[15] + 4 + inst_cream->imm;
+        INC_PC(sizeof(b_2_thumb));
+        goto DISPATCH;
+    }
+    B_COND_THUMB:
+    {
+        b_cond_thumb *inst_cream = (b_cond_thumb *)inst_base->component;
+
+        if(CondPassed(cpu, inst_cream->cond))
+            cpu->Reg[15] = cpu->Reg[15] + 4 + inst_cream->imm;
+        else
+            cpu->Reg[15] += 2;
+
+        INC_PC(sizeof(b_cond_thumb));
+        goto DISPATCH;
+    }
+    BL_1_THUMB:
+    {
+        bl_1_thumb *inst_cream = (bl_1_thumb *)inst_base->component;
+        cpu->Reg[14] = cpu->Reg[15] + 4 + inst_cream->imm;
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(bl_1_thumb));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+    BL_2_THUMB:
+    {
+        bl_2_thumb *inst_cream = (bl_2_thumb *)inst_base->component;
+        int tmp = ((cpu->Reg[15] + 2) | 1);
+        cpu->Reg[15] = (cpu->Reg[14] + inst_cream->imm);
+        cpu->Reg[14] = tmp;
+        INC_PC(sizeof(bl_2_thumb));
+        goto DISPATCH;
+    }
+    BLX_1_THUMB:
+    {
+        // BLX 1 for armv5t and above
+        uint32 tmp = cpu->Reg[15];
+        blx_1_thumb *inst_cream = (blx_1_thumb *)inst_base->component;
+        cpu->Reg[15] = (cpu->Reg[14] + inst_cream->imm) & 0xFFFFFFFC;
+        cpu->Reg[14] = ((tmp + 2) | 1);
+        cpu->TFlag = 0;
+        INC_PC(sizeof(blx_1_thumb));
+        goto DISPATCH;
+    }
+
+    UQADD8_INST:
+    UQADD16_INST:
+    UQADDSUBX_INST:
+    UQSUB8_INST:
+    UQSUB16_INST:
+    UQSUBADDX_INST:
+    {
+        if (inst_base->cond == 0xE || CondPassed(cpu, inst_base->cond)) {
+            generic_arm_inst* const inst_cream = (generic_arm_inst*)inst_base->component;
+
+            const u8 op2 = inst_cream->op2;
+            const u32 rm_val = RM;
+            const u32 rn_val = RN;
+
+            u16 lo_val = 0;
+            u16 hi_val = 0;
+            
+            // UQADD16
+            if (op2 == 0x00) {
+                lo_val = ARMul_UnsignedSaturatedAdd16(rn_val & 0xFFFF, rm_val & 0xFFFF);
+                hi_val = ARMul_UnsignedSaturatedAdd16((rn_val >> 16) & 0xFFFF, (rm_val >> 16) & 0xFFFF);
+            }
+            // UQASX
+            else if (op2 == 0x01) {
+                lo_val = ARMul_UnsignedSaturatedSub16(rn_val & 0xFFFF, (rm_val >> 16) & 0xFFFF);
+                hi_val = ARMul_UnsignedSaturatedAdd16((rn_val >> 16) & 0xFFFF, rm_val & 0xFFFF);
+            }
+            // UQSAX
+            else if (op2 == 0x02) {
+                lo_val = ARMul_UnsignedSaturatedAdd16(rn_val & 0xFFFF, (rm_val >> 16) & 0xFFFF);
+                hi_val = ARMul_UnsignedSaturatedSub16((rn_val >> 16) & 0xFFFF, rm_val & 0xFFFF);
+            }
+            // UQSUB16
+            else if (op2 == 0x03) {
+                lo_val = ARMul_UnsignedSaturatedSub16(rn_val & 0xFFFF, rm_val & 0xFFFF);
+                hi_val = ARMul_UnsignedSaturatedSub16((rn_val >> 16) & 0xFFFF, (rm_val >> 16) & 0xFFFF);
+            }
+            // UQADD8
+            else if (op2 == 0x04) {
+                lo_val = ARMul_UnsignedSaturatedAdd8(rn_val, rm_val) |
+                         ARMul_UnsignedSaturatedAdd8(rn_val >> 8,  rm_val >> 8) << 8;
+                hi_val = ARMul_UnsignedSaturatedAdd8(rn_val >> 16, rm_val >> 16) |
+                         ARMul_UnsignedSaturatedAdd8(rn_val >> 24, rm_val >> 24) << 8;
+            }
+            // UQSUB8
+            else {
+                lo_val = ARMul_UnsignedSaturatedSub8(rn_val, rm_val) |
+                         ARMul_UnsignedSaturatedSub8(rn_val >> 8,  rm_val >> 8) << 8;
+                hi_val = ARMul_UnsignedSaturatedSub8(rn_val >> 16, rm_val >> 16) |
+                         ARMul_UnsignedSaturatedSub8(rn_val >> 24, rm_val >> 24) << 8;
+            }
+
+            RD = ((lo_val & 0xFFFF) | hi_val << 16);
+        }
+
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(generic_arm_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+
+    USAD8_INST:
+    USADA8_INST:
+    {
+        if (inst_base->cond == 0xE || CondPassed(cpu, inst_base->cond)) {
+            generic_arm_inst* inst_cream = (generic_arm_inst*)inst_base->component;
+
+            const u8 ra_idx = inst_cream->Ra;
+            const u32 rm_val = RM;
+            const u32 rn_val = RN;
+
+            const u8 diff1 = ARMul_UnsignedAbsoluteDifference(rn_val & 0xFF, rm_val & 0xFF);
+            const u8 diff2 = ARMul_UnsignedAbsoluteDifference((rn_val >> 8) & 0xFF, (rm_val >> 8) & 0xFF);
+            const u8 diff3 = ARMul_UnsignedAbsoluteDifference((rn_val >> 16) & 0xFF, (rm_val >> 16) & 0xFF);
+            const u8 diff4 = ARMul_UnsignedAbsoluteDifference((rn_val >> 24) & 0xFF, (rm_val >> 24) & 0xFF);
+
+            u32 finalDif = (diff1 + diff2 + diff3 + diff4);
+
+            // Op is USADA8 if true.
+            if (ra_idx != 15)
+                finalDif += cpu->Reg[ra_idx];
+
+            RD = finalDif;
+        }
+
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(generic_arm_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+
+    USAT_INST:
+    USAT16_INST:
+    USUB16_INST:
+    USUB8_INST:
+    USUBADDX_INST:
+    UXTAB16_INST:
+    UXTB16_INST:
+    {
+        if (inst_base->cond == 0xE || CondPassed(cpu, inst_base->cond)) {
+            uxtab_inst* const inst_cream = (uxtab_inst*)inst_base->component;
+
+            const u8 rn_idx = inst_cream->Rn;
+            const u32 rm_val = RM;
+            const u32 rotation = inst_cream->rotate * 8;
+            const u32 rotated_rm = ((rm_val << (32 - rotation)) | (rm_val >> rotation));
+
+            // UXTB16, otherwise UXTAB16
+            if (rn_idx == 15) {
+                RD = rotated_rm & 0x00FF00FF;
+            } else {
+                const u32 rn_val = RN;
+                const u8 lo_rotated = (rotated_rm & 0xFF);
+                const u16 lo_result = (rn_val & 0xFFFF) + (u16)lo_rotated;
+                const u8 hi_rotated = (rotated_rm >> 16) & 0xFF;
+                const u16 hi_result = (rn_val >> 16) + (u16)hi_rotated;
+
+                RD = ((hi_result << 16) | (lo_result & 0xFFFF));
+            }
+        }
+
+        cpu->Reg[15] += GET_INST_SIZE(cpu);
+        INC_PC(sizeof(uxtab_inst));
+        FETCH_INST;
+        GOTO_NEXT_INST;
+    }
+
+    #define VFP_INTERPRETER_IMPL
+    #include "core/arm/skyeye_common/vfp/vfpinstr.cpp"
+    #undef VFP_INTERPRETER_IMPL
+    MMU_EXCEPTION:
+    {
+        SAVE_NZCVT;
+        cpu->abortSig = true;
+        cpu->Aborted = ARMul_DataAbortV;
+        cpu->AbortAddr = addr;
+        cpu->CP15[CP15(CP15_FAULT_STATUS)] = fault & 0xff;
+        cpu->CP15[CP15(CP15_FAULT_ADDRESS)] = addr;
+        cpu->NumInstrsToExecute = 0;
+        return num_instrs;
+    }
+    END:
+    {
+        SAVE_NZCVT;
+        cpu->NumInstrsToExecute = 0;
+        return num_instrs;
+    }
+    INIT_INST_LENGTH:
+    {
 #if defined __GNUC__ || defined __clang__
-        InterpreterInitInstLength((unsigned long long int *)InstLabel, sizeof(InstLabel));
+        InterpreterInitInstLength((unsigned long long int *)InstLabel, sizeof(InstLabel));
 #endif
-#if 0
-        for (int i = 0; i < (sizeof(InstLabel) / sizeof(void *)); i ++)
-                DEBUG_LOG(ARM11, "[%llx]\n", InstLabel[i]);
-        DEBUG_LOG(ARM11, "%llx\n", InstEndLabel[1]);
-        DEBUG_LOG(ARM11, "%llx\n", InstLabel[1]);
-        DEBUG_LOG(ARM11, "%lld\n", (char *)InstEndLabel[1] - (char *)InstLabel[1]);
-#endif
-        cpu->NumInstrsToExecute = 0;
-        return num_instrs;
-        }
+        cpu->NumInstrsToExecute = 0;
+        return num_instrs;
+    }
 }
-
diff --git a/src/core/arm/dyncom/arm_dyncom_interpreter.h b/src/core/arm/dyncom/arm_dyncom_interpreter.h
index 3a2462f55..4791ea25f 100644
--- a/src/core/arm/dyncom/arm_dyncom_interpreter.h
+++ b/src/core/arm/dyncom/arm_dyncom_interpreter.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/core/arm/dyncom/arm_dyncom_run.cpp b/src/core/arm/dyncom/arm_dyncom_run.cpp
index a2026cbf3..d457d0ac5 100644
--- a/src/core/arm/dyncom/arm_dyncom_run.cpp
+++ b/src/core/arm/dyncom/arm_dyncom_run.cpp
@@ -1,42 +1,15 @@
-/* Copyright (C)
-* 2011 - Michael.Kang blackfin.kang@gmail.com
-* This program is free software; you can redistribute it and/or
-* modify it under the terms of the GNU General Public License
-* as published by the Free Software Foundation; either version 2
-* of the License, or (at your option) any later version.
-*
-* This program is distributed in the hope that 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, write to the Free Software
-* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
-*
-*/
-/**
-* @file arm_dyncom_run.cpp
-* @brief The dyncom run implementation for arm
-* @author Michael.Kang blackfin.kang@gmail.com
-* @version 78.77
-* @date 2011-11-20
-*/
+// Copyright 2012 Michael Kang, 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
 
 #include <assert.h>
 
 #include "core/arm/skyeye_common/armdefs.h"
 
-void switch_mode(arm_core_t *core, uint32_t mode)
-{
-    uint32_t tmp1, tmp2;
-    if (core->Mode == mode) {
-        //Mode not changed.
-        //printf("mode not changed\n");
+void switch_mode(arm_core_t *core, uint32_t mode) {
+    if (core->Mode == mode)
         return;
-    }
-    //printf("%d --->>> %d\n", core->Mode, mode);
-    //printf("In %s, Cpsr=0x%x, R15=0x%x, last_pc=0x%x, cpsr=0x%x, spsr_copy=0x%x, icounter=%lld\n", __FUNCTION__, core->Cpsr, core->Reg[15], core->last_pc, core->Cpsr, core->Spsr_copy, core->icounter);
+
     if (mode != USERBANK) {
         switch (core->Mode) {
         case USER32MODE:
@@ -110,11 +83,8 @@ void switch_mode(arm_core_t *core, uint32_t mode)
 
         }
         core->Mode = mode;
-        //printf("In %si end, Cpsr=0x%x, R15=0x%x, last_pc=0x%x, cpsr=0x%x, spsr_copy=0x%x, icounter=%lld\n", __FUNCTION__, core->Cpsr, core->Reg[15], core->last_pc, core->Cpsr, core->Spsr_copy, core->icounter);
-        //printf("\n--------------------------------------\n");
-    }
-    else {
-        printf("user mode\n");
+    } else {
+        LOG_CRITICAL(Core_ARM11, "user mode");
         exit(-2);
     }
 }
diff --git a/src/core/arm/dyncom/arm_dyncom_thumb.cpp b/src/core/arm/dyncom/arm_dyncom_thumb.cpp
index e10f2f9ee..de70ca8ae 100644
--- a/src/core/arm/dyncom/arm_dyncom_thumb.cpp
+++ b/src/core/arm/dyncom/arm_dyncom_thumb.cpp
@@ -1,35 +1,13 @@
-/* Copyright (C) 
-* 2011 - Michael.Kang blackfin.kang@gmail.com
-* This program is free software; you can redistribute it and/or
-* modify it under the terms of the GNU General Public License
-* as published by the Free Software Foundation; either version 2
-* of the License, or (at your option) any later version.
-* 
-* This program is distributed in the hope that 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, write to the Free Software
-* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
-* 
-*/
-/**
-* @file arm_dyncom_thumb.c
-* @brief The thumb dynamic interpreter
-* @author Michael.Kang blackfin.kang@gmail.com
-* @version 78.77
-* @date 2011-11-07
-*/
-
-/* We can provide simple Thumb simulation by decoding the Thumb
-instruction into its corresponding ARM instruction, and using the
-existing ARM simulator.  */
+// Copyright 2012 Michael Kang, 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+// We can provide simple Thumb simulation by decoding the Thumb instruction into its corresponding
+// ARM instruction, and using the existing ARM simulator.
 
 #include "core/arm/skyeye_common/skyeye_defs.h"
 
-#ifndef MODET                   /* required for the Thumb instruction support */
+#ifndef MODET // Required for the Thumb instruction support
 #if 1
 #error "MODET needs to be defined for the Thumb world to work"
 #else
@@ -40,482 +18,359 @@ existing ARM simulator.  */
 #include "core/arm/skyeye_common/armos.h"
 #include "core/arm/dyncom/arm_dyncom_thumb.h"
 
-/* Decode a 16bit Thumb instruction.  The instruction is in the low
-   16-bits of the tinstr field, with the following Thumb instruction
-   held in the high 16-bits.  Passing in two Thumb instructions allows
-   easier simulation of the special dual BL instruction.  */
+// Decode a 16bit Thumb instruction.  The instruction is in the low 16-bits of the tinstr field,
+// with the following Thumb instruction held in the high 16-bits.  Passing in two Thumb instructions
+// allows easier simulation of the special dual BL instruction.
 
-tdstate thumb_translate (addr_t addr, uint32_t instr, uint32_t* ainstr, uint32_t* inst_size)
-{
+tdstate thumb_translate (addr_t addr, uint32_t instr, uint32_t* ainstr, uint32_t* inst_size) {
     tdstate valid = t_uninitialized;
-        ARMword next_instr;
-        ARMword tinstr;
-        tinstr = instr;
-        /* The endian should be judge here */
-        #if 0
-        if (state->bigendSig) {
-                next_instr = tinstr & 0xFFFF;
-                tinstr >>= 16;
-        }
-        else {
-                next_instr = tinstr >> 16;
-                tinstr &= 0xFFFF;
-        }
-        #endif
-        if((addr & 0x3) != 0)
-                tinstr = instr >> 16;
-        else
-                tinstr &= 0xFFFF;
-
-        //printf("In %s, instr=0x%x, tinstr=0x%x, r15=0x%x\n", __FUNCTION__, instr, tinstr, cpu->translate_pc);
-#if 1                           /* debugging to catch non updates */
-        *ainstr = 0xDEADC0DE;
-#endif
+    ARMword tinstr;
+    tinstr = instr;
+
+    // The endian should be judge here
+    if((addr & 0x3) != 0)
+        tinstr = instr >> 16;
+    else
+        tinstr &= 0xFFFF;
+
+    *ainstr = 0xDEADC0DE; // Debugging to catch non updates
+
+    switch ((tinstr & 0xF800) >> 11) {
+    case 0: // LSL
+    case 1: // LSR
+    case 2: // ASR
+        *ainstr = 0xE1B00000                    // base opcode
+            | ((tinstr & 0x1800) >> (11 - 5))   // shift type
+            |((tinstr & 0x07C0) << (7 - 6))     // imm5
+            |((tinstr & 0x0038) >> 3)           // Rs
+            |((tinstr & 0x0007) << 12);         // Rd
+        break;
+
+    case 3: // ADD/SUB
+        {
+            ARMword subset[4] = {
+                0xE0900000,     // ADDS Rd,Rs,Rn
+                0xE0500000,     // SUBS Rd,Rs,Rn
+                0xE2900000,     // ADDS Rd,Rs,#imm3
+                0xE2500000      // SUBS Rd,Rs,#imm3
+            };
+            // It is quicker indexing into a table, than performing switch or conditionals:
+            *ainstr = subset[(tinstr & 0x0600) >> 9]    // base opcode
+                |((tinstr & 0x01C0) >> 6)               // Rn or imm3
+                |((tinstr & 0x0038) << (16 - 3))        // Rs
+                |((tinstr & 0x0007) << (12 - 0));       // Rd
+        }
+        break;
+
+    case 4: // MOV
+    case 5: // CMP
+    case 6: // ADD
+    case 7: // SUB
+        {
+            ARMword subset[4] = {
+                0xE3B00000,     // MOVS Rd,#imm8
+                0xE3500000,     // CMP  Rd,#imm8
+                0xE2900000,     // ADDS Rd,Rd,#imm8
+                0xE2500000,     // SUBS Rd,Rd,#imm8
+            };
+
+            *ainstr = subset[(tinstr & 0x1800) >> 11]   // base opcode
+                |((tinstr & 0x00FF) >> 0)               // imm8
+                |((tinstr & 0x0700) << (16 - 8))        // Rn
+                |((tinstr & 0x0700) << (12 - 8));       // Rd
+        }
+        break;
+
+    case 8: // Arithmetic and high register transfers
+
+        // TODO: Since the subsets for both Format 4 and Format 5 instructions are made up of
+        // different ARM encodings, we could save the following conditional, and just have one
+        // large subset
+
+        if ((tinstr & (1 << 10)) == 0) {
+            enum otype {
+                t_norm,
+                t_shift,
+                t_neg,
+                t_mul
+            };
+
+            struct {
+                ARMword opcode;
+                otype type;
+            } subset[16] = {
+                { 0xE0100000, t_norm },     // ANDS Rd,Rd,Rs
+                { 0xE0300000, t_norm },     // EORS Rd,Rd,Rs
+                { 0xE1B00010, t_shift },    // MOVS Rd,Rd,LSL Rs
+                { 0xE1B00030, t_shift },    // MOVS Rd,Rd,LSR Rs
+                { 0xE1B00050, t_shift },    // MOVS Rd,Rd,ASR Rs
+                { 0xE0B00000, t_norm },     // ADCS Rd,Rd,Rs
+                { 0xE0D00000, t_norm },     // SBCS Rd,Rd,Rs
+                { 0xE1B00070, t_shift },    // MOVS Rd,Rd,ROR Rs
+                { 0xE1100000, t_norm },     // TST  Rd,Rs
+                { 0xE2700000, t_neg },      // RSBS Rd,Rs,#0
+                { 0xE1500000, t_norm },     // CMP  Rd,Rs
+                { 0xE1700000, t_norm },     // CMN  Rd,Rs
+                { 0xE1900000, t_norm },     // ORRS Rd,Rd,Rs
+                { 0xE0100090, t_mul },      // MULS Rd,Rd,Rs
+                { 0xE1D00000, t_norm },     // BICS Rd,Rd,Rs
+                { 0xE1F00000, t_norm }      // MVNS Rd,Rs
+            };
+
+            *ainstr = subset[(tinstr & 0x03C0) >> 6].opcode; // base
+
+            switch (subset[(tinstr & 0x03C0) >> 6].type) {
+            case t_norm:
+                *ainstr |= ((tinstr & 0x0007) << 16)    // Rn
+                    |((tinstr & 0x0007) << 12)          // Rd
+                    |((tinstr & 0x0038) >> 3);          // Rs
+                break;
+            case t_shift:
+                *ainstr |= ((tinstr & 0x0007) << 12)    // Rd
+                    |((tinstr & 0x0007) >> 0)           // Rm
+                    |((tinstr & 0x0038) << (8 - 3));    // Rs
+                break;
+            case t_neg:
+                *ainstr |= ((tinstr & 0x0007) << 12)    // Rd
+                    |((tinstr & 0x0038) << (16 - 3));   // Rn
+                break;
+            case t_mul:
+                *ainstr |= ((tinstr & 0x0007) << 16)    // Rd
+                    |((tinstr & 0x0007) << 8)           // Rs
+                    |((tinstr & 0x0038) >> 3);          // Rm
+                break;
+            }
+        } else {
+            ARMword Rd = ((tinstr & 0x0007) >> 0);
+            ARMword Rs = ((tinstr & 0x0038) >> 3);
+
+            if (tinstr & (1 << 7))
+                Rd += 8;
+            if (tinstr & (1 << 6))
+                Rs += 8;
+
+            switch ((tinstr & 0x03C0) >> 6) {
+            case 0x1:                           // ADD Rd,Rd,Hs
+            case 0x2:                           // ADD Hd,Hd,Rs
+            case 0x3:                           // ADD Hd,Hd,Hs
+                *ainstr = 0xE0800000            // base
+                    | (Rd << 16)                // Rn
+                    |(Rd << 12)                 // Rd
+                    |(Rs << 0);                 // Rm
+                break;
+            case 0x5:                           // CMP Rd,Hs
+            case 0x6:                           // CMP Hd,Rs
+            case 0x7:                           // CMP Hd,Hs
+                *ainstr = 0xE1500000            // base
+                    | (Rd << 16)                // Rn
+                    |(Rd << 12)                 // Rd
+                    |(Rs << 0);                 // Rm
+                break;
+            case 0x9:                           // MOV Rd,Hs
+            case 0xA:                           // MOV Hd,Rs
+            case 0xB:                           // MOV Hd,Hs
+                *ainstr = 0xE1A00000            // base
+                    | (Rd << 16)                // Rn
+                    |(Rd << 12)                 // Rd
+                    |(Rs << 0);                 // Rm
+                break;
+            case 0xC:                           // BX Rs
+            case 0xD:                           // BX Hs
+                *ainstr = 0xE12FFF10            // base
+                    | ((tinstr & 0x0078) >> 3); // Rd
+                break;
+            case 0x0:                           // UNDEFINED
+            case 0x4:                           // UNDEFINED
+            case 0x8:                           // UNDEFINED
+                valid = t_undefined;
+                break;
+            case 0xE:                           // BLX
+            case 0xF:                           // BLX
+                *ainstr = 0xE1200030            // base
+                    | (Rs << 0);                // Rm
+                break;
+            }
+        }
+        break;
+
+    case 9: // LDR Rd,[PC,#imm8]
+        *ainstr = 0xE59F0000                    // base
+            | ((tinstr & 0x0700) << (12 - 8))   // Rd
+            |((tinstr & 0x00FF) << (2 - 0));    // off8 
+        break;
+
+    case 10:
+    case 11:
+        // TODO: Format 7 and Format 8 perform the same ARM encoding, so the following could be
+        // merged into a single subset, saving on the following boolean:
+
+        if ((tinstr & (1 << 9)) == 0) {
+            ARMword subset[4] = {
+                0xE7800000, // STR  Rd,[Rb,Ro]
+                0xE7C00000, // STRB Rd,[Rb,Ro]
+                0xE7900000, // LDR  Rd,[Rb,Ro]
+                0xE7D00000  // LDRB Rd,[Rb,Ro]
+            };
+
+            *ainstr = subset[(tinstr & 0x0C00) >> 10]   // base
+                |((tinstr & 0x0007) << (12 - 0))        // Rd
+                |((tinstr & 0x0038) << (16 - 3))        // Rb
+                |((tinstr & 0x01C0) >> 6);              // Ro
+
+        } else {
+            ARMword subset[4] = {
+                0xE18000B0, // STRH  Rd,[Rb,Ro]
+                0xE19000D0, // LDRSB Rd,[Rb,Ro]
+                0xE19000B0, // LDRH  Rd,[Rb,Ro]
+                0xE19000F0  // LDRSH Rd,[Rb,Ro]
+            };
+            *ainstr = subset[(tinstr & 0x0C00) >> 10]   // base
+                |((tinstr & 0x0007) << (12 - 0))        // Rd
+                |((tinstr & 0x0038) << (16 - 3))        // Rb
+                |((tinstr & 0x01C0) >> 6);              // Ro
+        }
+        break;
+
+    case 12: // STR Rd,[Rb,#imm5]
+    case 13: // LDR Rd,[Rb,#imm5]
+    case 14: // STRB Rd,[Rb,#imm5]
+    case 15: // LDRB Rd,[Rb,#imm5]
+        {
+            ARMword subset[4] = {
+                0xE5800000,     // STR  Rd,[Rb,#imm5]
+                0xE5900000,     // LDR  Rd,[Rb,#imm5]
+                0xE5C00000,     // STRB Rd,[Rb,#imm5]
+                0xE5D00000      // LDRB Rd,[Rb,#imm5]
+            };
+            // The offset range defends on whether we are transferring a byte or word value:
+            *ainstr = subset[(tinstr & 0x1800) >> 11]   // base
+                |((tinstr & 0x0007) << (12 - 0))        // Rd
+                |((tinstr & 0x0038) << (16 - 3))        // Rb
+                |((tinstr & 0x07C0) >> (6 - ((tinstr & (1 << 12)) ? 0 : 2))); // off5
+        }
+        break;
+
+    case 16: // STRH Rd,[Rb,#imm5]
+    case 17: // LDRH Rd,[Rb,#imm5]
+        *ainstr = ((tinstr & (1 << 11))         // base
+               ? 0xE1D000B0                     // LDRH
+               : 0xE1C000B0)                    // STRH
+            |((tinstr & 0x0007) << (12 - 0))    // Rd
+            |((tinstr & 0x0038) << (16 - 3))    // Rb
+            |((tinstr & 0x01C0) >> (6 - 1))     // off5, low nibble
+            |((tinstr & 0x0600) >> (9 - 8));    // off5, high nibble
+        break;
+
+    case 18: // STR Rd,[SP,#imm8]
+    case 19: // LDR Rd,[SP,#imm8]
+        *ainstr = ((tinstr & (1 << 11))         // base
+               ? 0xE59D0000                     // LDR
+               : 0xE58D0000)                    // STR
+            |((tinstr & 0x0700) << (12 - 8))    // Rd
+            |((tinstr & 0x00FF) << 2);          // off8
+        break;
+
+    case 20: // ADD Rd,PC,#imm8
+    case 21: // ADD Rd,SP,#imm8
+
+        if ((tinstr & (1 << 11)) == 0) {
+
+            // NOTE: The PC value used here should by word aligned. We encode shift-left-by-2 in the
+            // rotate immediate field, so no shift of off8 is needed.
+
+            *ainstr = 0xE28F0F00                    // base
+                | ((tinstr & 0x0700) << (12 - 8))   // Rd
+                |(tinstr & 0x00FF);                 // off8
+        } else {
+            // We encode shift-left-by-2 in the rotate immediate field, so no shift of off8 is needed.
+            *ainstr = 0xE28D0F00                    // base
+                | ((tinstr & 0x0700) << (12 - 8))   // Rd
+                |(tinstr & 0x00FF);                 // off8
+        }
+        break;
+
+    case 22:
+    case 23:
+        if ((tinstr & 0x0F00) == 0x0000) {
+            // NOTE: The instruction contains a shift left of 2 equivalent (implemented as ROR #30):
+            *ainstr = ((tinstr & (1 << 7))  // base
+                   ? 0xE24DDF00             // SUB
+                   : 0xE28DDF00)            // ADD
+                |(tinstr & 0x007F);         // off7
+        } else if ((tinstr & 0x0F00) == 0x0e00)
+            *ainstr = 0xEF000000 | SWI_Breakpoint;
+        else {
+            ARMword subset[4] = {
+                0xE92D0000, // STMDB sp!,{rlist}
+                0xE92D4000, // STMDB sp!,{rlist,lr}
+                0xE8BD0000, // LDMIA sp!,{rlist}
+                0xE8BD8000  // LDMIA sp!,{rlist,pc}
+            };
+            *ainstr = subset[((tinstr & (1 << 11)) >> 10) | ((tinstr & (1 << 8)) >> 8)] // base
+                |(tinstr & 0x00FF); // mask8
+        }
+        break;
+
+    case 24: //  STMIA
+    case 25: //  LDMIA
+        *ainstr = ((tinstr & (1 << 11))         // base
+               ? 0xE8B00000                     // LDMIA
+               : 0xE8A00000)                    // STMIA
+            |((tinstr & 0x0700) << (16 - 8))    // Rb
+            |(tinstr & 0x00FF);                 // mask8
+        break;
+
+    case 26: // Bcc
+    case 27: // Bcc/SWI
+        if ((tinstr & 0x0F00) == 0x0F00) {
+            // Format 17 : SWI
+            *ainstr = 0xEF000000;
+            // Breakpoint must be handled specially.
+            if ((tinstr & 0x00FF) == 0x18)
+                *ainstr |= ((tinstr & 0x00FF) << 16);
+            // New breakpoint value.  See gdb/arm-tdep.c
+            else if ((tinstr & 0x00FF) == 0xFE)
+                *ainstr |= SWI_Breakpoint;
+            else
+                *ainstr |= (tinstr & 0x00FF);
+        } else if ((tinstr & 0x0F00) != 0x0E00)
+            valid = t_branch;
+        else //  UNDEFINED : cc=1110(AL) uses different format
+            valid = t_undefined;
+
+        break;
+
+    case 28: // B
+        valid = t_branch;
+        break;
+
+    case 29:
+        if(tinstr & 0x1)
+            valid = t_undefined;
+        else
+            valid = t_branch;
+        break;
+
+    case 30: // BL instruction 1
+
+        // There is no single ARM instruction equivalent for this Thumb instruction. To keep the
+        // simulation simple (from the user perspective) we check if the following instruction is
+        // the second half of this BL, and if it is we simulate it immediately
+
+        valid = t_branch;
+        break;
+
+    case 31: // BL instruction 2
+
+        // There is no single ARM instruction equivalent for this instruction. Also, it should only
+        // ever be matched with the fmt19 "BL instruction 1" instruction. However, we do allow the
+        // simulation of it on its own, with undefined results if r14 is not suitably initialised.
+
+        valid = t_branch;
+        break;
+    }
+
+    *inst_size = 2;
 
-        switch ((tinstr & 0xF800) >> 11) {
-        case 0:         /* LSL */
-        case 1:         /* LSR */
-        case 2:         /* ASR */
-                /* Format 1 */
-                *ainstr = 0xE1B00000    /* base opcode */
-                        | ((tinstr & 0x1800) >> (11 - 5))       /* shift type */
-                        |((tinstr & 0x07C0) << (7 - 6)) /* imm5 */
-                        |((tinstr & 0x0038) >> 3)       /* Rs */
-                        |((tinstr & 0x0007) << 12);     /* Rd */
-                break;
-        case 3:         /* ADD/SUB */
-                /* Format 2 */
-                {
-                        ARMword subset[4] = {
-                                0xE0900000,     /* ADDS Rd,Rs,Rn    */
-                                0xE0500000,     /* SUBS Rd,Rs,Rn    */
-                                0xE2900000,     /* ADDS Rd,Rs,#imm3 */
-                                0xE2500000      /* SUBS Rd,Rs,#imm3 */
-                        };
-                        /* It is quicker indexing into a table, than performing switch
-                           or conditionals: */
-                        *ainstr = subset[(tinstr & 0x0600) >> 9]        /* base opcode */
-                                |((tinstr & 0x01C0) >> 6)       /* Rn or imm3 */
-                                |((tinstr & 0x0038) << (16 - 3))        /* Rs */
-                                |((tinstr & 0x0007) << (12 - 0));       /* Rd */
-                }
-                break;
-        case 4:         /* MOV */
-        case 5:         /* CMP */
-        case 6:         /* ADD */
-        case 7:         /* SUB */
-                /* Format 3 */
-                {
-                        ARMword subset[4] = {
-                                0xE3B00000,     /* MOVS Rd,#imm8    */
-                                0xE3500000,     /* CMP  Rd,#imm8    */
-                                0xE2900000,     /* ADDS Rd,Rd,#imm8 */
-                                0xE2500000,     /* SUBS Rd,Rd,#imm8 */
-                        };
-                        *ainstr = subset[(tinstr & 0x1800) >> 11]       /* base opcode */
-                                |((tinstr & 0x00FF) >> 0)       /* imm8 */
-                                |((tinstr & 0x0700) << (16 - 8))        /* Rn */
-                                |((tinstr & 0x0700) << (12 - 8));       /* Rd */
-                }
-                break;
-        case 8:         /* Arithmetic and high register transfers */
-                /* TODO: Since the subsets for both Format 4 and Format 5
-                   instructions are made up of different ARM encodings, we could
-                   save the following conditional, and just have one large
-                   subset. */
-                if ((tinstr & (1 << 10)) == 0) {
-                        typedef enum
-                        { t_norm, t_shift, t_neg, t_mul }otype_t;
-
-                        /* Format 4 */
-                        struct
-                        {
-                                ARMword opcode;
-                                otype_t otype;
-                        }
-                        subset[16] = {
-                                {
-                                0xE0100000, t_norm},    /* ANDS Rd,Rd,Rs     */
-                                {
-                                0xE0300000, t_norm},    /* EORS Rd,Rd,Rs     */
-                                {
-                                0xE1B00010, t_shift},   /* MOVS Rd,Rd,LSL Rs */
-                                {
-                                0xE1B00030, t_shift},   /* MOVS Rd,Rd,LSR Rs */
-                                {
-                                0xE1B00050, t_shift},   /* MOVS Rd,Rd,ASR Rs */
-                                {
-                                0xE0B00000, t_norm},    /* ADCS Rd,Rd,Rs     */
-                                {
-                                0xE0D00000, t_norm},    /* SBCS Rd,Rd,Rs     */
-                                {
-                                0xE1B00070, t_shift},   /* MOVS Rd,Rd,ROR Rs */
-                                {
-                                0xE1100000, t_norm},    /* TST  Rd,Rs        */
-                                {
-                                0xE2700000, t_neg},     /* RSBS Rd,Rs,#0     */
-                                {
-                                0xE1500000, t_norm},    /* CMP  Rd,Rs        */
-                                {
-                                0xE1700000, t_norm},    /* CMN  Rd,Rs        */
-                                {
-                                0xE1900000, t_norm},    /* ORRS Rd,Rd,Rs     */
-                                {
-                                0xE0100090, t_mul},     /* MULS Rd,Rd,Rs     */
-                                {
-                                0xE1D00000, t_norm},    /* BICS Rd,Rd,Rs     */
-                                {
-                                0xE1F00000, t_norm}     /* MVNS Rd,Rs        */
-                        };
-                        *ainstr = subset[(tinstr & 0x03C0) >> 6].opcode;        /* base */
-                        switch (subset[(tinstr & 0x03C0) >> 6].otype) {
-                        case t_norm:
-                                *ainstr |= ((tinstr & 0x0007) << 16)    /* Rn */
-                                        |((tinstr & 0x0007) << 12)      /* Rd */
-                                        |((tinstr & 0x0038) >> 3);      /* Rs */
-                                break;
-                        case t_shift:
-                                *ainstr |= ((tinstr & 0x0007) << 12)    /* Rd */
-                                        |((tinstr & 0x0007) >> 0)       /* Rm */
-                                        |((tinstr & 0x0038) << (8 - 3));        /* Rs */
-                                break;
-                        case t_neg:
-                                *ainstr |= ((tinstr & 0x0007) << 12)    /* Rd */
-                                        |((tinstr & 0x0038) << (16 - 3));       /* Rn */
-                                break;
-                        case t_mul:
-                                *ainstr |= ((tinstr & 0x0007) << 16)    /* Rd */
-                                        |((tinstr & 0x0007) << 8)       /* Rs */
-                                        |((tinstr & 0x0038) >> 3);      /* Rm */
-                                break;
-                        }
-                }
-                else {
-                        /* Format 5 */
-                        ARMword Rd = ((tinstr & 0x0007) >> 0);
-                        ARMword Rs = ((tinstr & 0x0038) >> 3);
-                        if (tinstr & (1 << 7))
-                                Rd += 8;
-                        if (tinstr & (1 << 6))
-                                Rs += 8;
-                        switch ((tinstr & 0x03C0) >> 6) {
-                        case 0x1:       /* ADD Rd,Rd,Hs */
-                        case 0x2:       /* ADD Hd,Hd,Rs */
-                        case 0x3:       /* ADD Hd,Hd,Hs */
-                                *ainstr = 0xE0800000    /* base */
-                                        | (Rd << 16)    /* Rn */
-                                        |(Rd << 12)     /* Rd */
-                                        |(Rs << 0);     /* Rm */
-                                break;
-                        case 0x5:       /* CMP Rd,Hs */
-                        case 0x6:       /* CMP Hd,Rs */
-                        case 0x7:       /* CMP Hd,Hs */
-                                *ainstr = 0xE1500000    /* base */
-                                        | (Rd << 16)    /* Rn */
-                                        |(Rd << 12)     /* Rd */
-                                        |(Rs << 0);     /* Rm */
-                                break;
-                        case 0x9:       /* MOV Rd,Hs */
-                        case 0xA:       /* MOV Hd,Rs */
-                        case 0xB:       /* MOV Hd,Hs */
-                                *ainstr = 0xE1A00000    /* base */
-                                        | (Rd << 16)    /* Rn */
-                                        |(Rd << 12)     /* Rd */
-                                        |(Rs << 0);     /* Rm */
-                                break;
-                        case 0xC:       /* BX Rs */
-                        case 0xD:       /* BX Hs */
-                                *ainstr = 0xE12FFF10    /* base */
-                                        | ((tinstr & 0x0078) >> 3);     /* Rd */
-                                break;
-                        case 0x0:       /* UNDEFINED */
-                        case 0x4:       /* UNDEFINED */
-                        case 0x8:       /* UNDEFINED */
-                                valid = t_undefined;
-                                break;
-                        case 0xE:       /* BLX */
-                        case 0xF:       /* BLX */
-                                
-                                //if (state->is_v5) {
-                                if(1){
-                                        //valid = t_branch;
-                                        #if 1
-                                        *ainstr = 0xE1200030    /* base */
-                                                |(Rs << 0);     /* Rm */
-                                        #endif
-                                } else {
-                                        valid = t_undefined;
-                                }
-                                break;
-                        }
-                }
-                break;
-        case 9:         /* LDR Rd,[PC,#imm8] */
-                /* Format 6 */
-                *ainstr = 0xE59F0000    /* base */
-                        | ((tinstr & 0x0700) << (12 - 8))       /* Rd */
-                        |((tinstr & 0x00FF) << (2 - 0));        /* off8 */
-                break;
-        case 10:
-        case 11:
-                /* TODO: Format 7 and Format 8 perform the same ARM encoding, so
-                   the following could be merged into a single subset, saving on
-                   the following boolean: */
-                if ((tinstr & (1 << 9)) == 0) {
-                        /* Format 7 */
-                        ARMword subset[4] = {
-                                0xE7800000,     /* STR  Rd,[Rb,Ro] */
-                                0xE7C00000,     /* STRB Rd,[Rb,Ro] */
-                                0xE7900000,     /* LDR  Rd,[Rb,Ro] */
-                                0xE7D00000      /* LDRB Rd,[Rb,Ro] */
-                        };
-                        *ainstr = subset[(tinstr & 0x0C00) >> 10]       /* base */
-                                |((tinstr & 0x0007) << (12 - 0))        /* Rd */
-                                |((tinstr & 0x0038) << (16 - 3))        /* Rb */
-                                |((tinstr & 0x01C0) >> 6);      /* Ro */
-                }
-                else {
-                        /* Format 8 */
-                        ARMword subset[4] = {
-                                0xE18000B0,     /* STRH  Rd,[Rb,Ro] */
-                                0xE19000D0,     /* LDRSB Rd,[Rb,Ro] */
-                                0xE19000B0,     /* LDRH  Rd,[Rb,Ro] */
-                                0xE19000F0      /* LDRSH Rd,[Rb,Ro] */
-                        };
-                        *ainstr = subset[(tinstr & 0x0C00) >> 10]       /* base */
-                                |((tinstr & 0x0007) << (12 - 0))        /* Rd */
-                                |((tinstr & 0x0038) << (16 - 3))        /* Rb */
-                                |((tinstr & 0x01C0) >> 6);      /* Ro */
-                }
-                break;
-        case 12:                /* STR Rd,[Rb,#imm5] */
-        case 13:                /* LDR Rd,[Rb,#imm5] */
-        case 14:                /* STRB Rd,[Rb,#imm5] */
-        case 15:                /* LDRB Rd,[Rb,#imm5] */
-                /* Format 9 */
-                {
-                        ARMword subset[4] = {
-                                0xE5800000,     /* STR  Rd,[Rb,#imm5] */
-                                0xE5900000,     /* LDR  Rd,[Rb,#imm5] */
-                                0xE5C00000,     /* STRB Rd,[Rb,#imm5] */
-                                0xE5D00000      /* LDRB Rd,[Rb,#imm5] */
-                        };
-                        /* The offset range defends on whether we are transferring a
-                           byte or word value: */
-                        *ainstr = subset[(tinstr & 0x1800) >> 11]       /* base */
-                                |((tinstr & 0x0007) << (12 - 0))        /* Rd */
-                                |((tinstr & 0x0038) << (16 - 3))        /* Rb */
-                                |((tinstr & 0x07C0) >> (6 - ((tinstr & (1 << 12)) ? 0 : 2)));   /* off5 */
-                }
-                break;
-        case 16:                /* STRH Rd,[Rb,#imm5] */
-        case 17:                /* LDRH Rd,[Rb,#imm5] */
-                /* Format 10 */
-                *ainstr = ((tinstr & (1 << 11)) /* base */
-                           ? 0xE1D000B0 /* LDRH */
-                           : 0xE1C000B0)        /* STRH */
-                        |((tinstr & 0x0007) << (12 - 0))        /* Rd */
-                        |((tinstr & 0x0038) << (16 - 3))        /* Rb */
-                        |((tinstr & 0x01C0) >> (6 - 1)) /* off5, low nibble */
-                        |((tinstr & 0x0600) >> (9 - 8));        /* off5, high nibble */
-                break;
-        case 18:                /* STR Rd,[SP,#imm8] */
-        case 19:                /* LDR Rd,[SP,#imm8] */
-                /* Format 11 */
-                *ainstr = ((tinstr & (1 << 11)) /* base */
-                           ? 0xE59D0000 /* LDR */
-                           : 0xE58D0000)        /* STR */
-                        |((tinstr & 0x0700) << (12 - 8))        /* Rd */
-                        |((tinstr & 0x00FF) << 2);      /* off8 */
-                break;
-        case 20:                /* ADD Rd,PC,#imm8 */
-        case 21:                /* ADD Rd,SP,#imm8 */
-                /* Format 12 */
-                if ((tinstr & (1 << 11)) == 0) {
-                        /* NOTE: The PC value used here should by word aligned */
-                        /* We encode shift-left-by-2 in the rotate immediate field,
-                           so no shift of off8 is needed.  */
-                        *ainstr = 0xE28F0F00    /* base */
-                                | ((tinstr & 0x0700) << (12 - 8))       /* Rd */
-                                |(tinstr & 0x00FF);     /* off8 */
-                }
-                else {
-                        /* We encode shift-left-by-2 in the rotate immediate field,
-                           so no shift of off8 is needed.  */
-                        *ainstr = 0xE28D0F00    /* base */
-                                | ((tinstr & 0x0700) << (12 - 8))       /* Rd */
-                                |(tinstr & 0x00FF);     /* off8 */
-                }
-                break;
-        case 22:
-        case 23:
-                if ((tinstr & 0x0F00) == 0x0000) {
-                        /* Format 13 */
-                        /* NOTE: The instruction contains a shift left of 2
-                           equivalent (implemented as ROR #30): */
-                        *ainstr = ((tinstr & (1 << 7))  /* base */
-                                   ? 0xE24DDF00 /* SUB */
-                                   : 0xE28DDF00)        /* ADD */
-                                |(tinstr & 0x007F);     /* off7 */
-                }
-                else if ((tinstr & 0x0F00) == 0x0e00)
-                        *ainstr = 0xEF000000 | SWI_Breakpoint;
-                else {
-                        /* Format 14 */
-                        ARMword subset[4] = {
-                                0xE92D0000,     /* STMDB sp!,{rlist}    */
-                                0xE92D4000,     /* STMDB sp!,{rlist,lr} */
-                                0xE8BD0000,     /* LDMIA sp!,{rlist}    */
-                                0xE8BD8000      /* LDMIA sp!,{rlist,pc} */
-                        };
-                        *ainstr = subset[((tinstr & (1 << 11)) >> 10) | ((tinstr & (1 << 8)) >> 8)]     /* base */
-                                |(tinstr & 0x00FF);     /* mask8 */
-                }
-                break;
-        case 24:                /* STMIA */
-        case 25:                /* LDMIA */
-                /* Format 15 */
-                *ainstr = ((tinstr & (1 << 11)) /* base */
-                           ? 0xE8B00000 /* LDMIA */
-                           : 0xE8A00000)        /* STMIA */
-                        |((tinstr & 0x0700) << (16 - 8))        /* Rb */
-                        |(tinstr & 0x00FF);     /* mask8 */
-                break;
-        case 26:                /* Bcc */
-        case 27:                /* Bcc/SWI */
-                if ((tinstr & 0x0F00) == 0x0F00) {
-                        #if 0
-                        if (tinstr == (ARMul_ABORTWORD & 0xffff) &&
-                                        state->AbortAddr == pc) {
-                                *ainstr = ARMul_ABORTWORD;
-                                break;
-                        }
-                        #endif
-                        /* Format 17 : SWI */
-                        *ainstr = 0xEF000000;
-                        /* Breakpoint must be handled specially.  */
-                        if ((tinstr & 0x00FF) == 0x18)
-                                *ainstr |= ((tinstr & 0x00FF) << 16);
-                        /* New breakpoint value.  See gdb/arm-tdep.c  */
-                        else if ((tinstr & 0x00FF) == 0xFE)
-                                *ainstr |= SWI_Breakpoint;
-                        else
-                                *ainstr |= (tinstr & 0x00FF);
-                }
-                else if ((tinstr & 0x0F00) != 0x0E00) {
-                        /* Format 16 */
-                        #if 0
-                        int doit = FALSE;
-                        /* TODO: Since we are doing a switch here, we could just add
-                           the SWI and undefined instruction checks into this
-                           switch to same on a couple of conditionals: */
-                        switch ((tinstr & 0x0F00) >> 8) {
-                        case EQ:
-                                doit = ZFLAG;
-                                break;
-                        case NE:
-                                doit = !ZFLAG;
-                                break;
-                        case VS:
-                                doit = VFLAG;
-                                break;
-                        case VC:
-                                doit = !VFLAG;
-                                break;
-                        case MI:
-                                doit = NFLAG;
-                                break;
-                        case PL:
-                                doit = !NFLAG;
-                                break;
-                        case CS:
-                                doit = CFLAG;
-                                break;
-                        case CC:
-                                doit = !CFLAG;
-                                break;
-                        case HI:
-                                doit = (CFLAG && !ZFLAG);
-                                break;
-                        case LS:
-                                doit = (!CFLAG || ZFLAG);
-                                break;
-                        case GE:
-                                doit = ((!NFLAG && !VFLAG)
-                                        || (NFLAG && VFLAG));
-                                break;
-                        case LT:
-                                doit = ((NFLAG && !VFLAG)
-                                        || (!NFLAG && VFLAG));
-                                break;
-                        case GT:
-                                doit = ((!NFLAG && !VFLAG && !ZFLAG)
-                                        || (NFLAG && VFLAG && !ZFLAG));
-                                break;
-                        case LE:
-                                doit = ((NFLAG && !VFLAG)
-                                        || (!NFLAG && VFLAG)) || ZFLAG;
-                                break;
-                        }
-                        if (doit) {
-                                state->Reg[15] = (pc + 4
-                                                  + (((tinstr & 0x7F) << 1)
-                                                     | ((tinstr & (1 << 7)) ?
-                                                        0xFFFFFF00 : 0)));
-                                FLUSHPIPE;
-                        }
-                        #endif
-                        valid = t_branch;
-                }
-                else            /* UNDEFINED : cc=1110(AL) uses different format */
-                        valid = t_undefined;
-                break;
-        case 28:                /* B */
-                /* Format 18 */
-                #if 0
-                state->Reg[15] = (pc + 4 + (((tinstr & 0x3FF) << 1)
-                                            | ((tinstr & (1 << 10)) ?
-                                               0xFFFFF800 : 0)));
-                #endif
-                //FLUSHPIPE;
-                valid = t_branch;
-                break;
-        case 29:
-                if(tinstr & 0x1)
-                        valid = t_undefined;
-                else{
-                        /* BLX 1 for armv5t and above */
-                        //printf("In %s, After  BLX(1),LR=0x%x,PC=0x%x, offset=0x%x\n", __FUNCTION__, state->Reg[14], state->Reg[15], (tinstr &0x7FF) << 1);
-                        valid = t_branch;
-                }
-                break;
-        case 30:                /* BL instruction 1 */
-                /* Format 19 */
-                /* There is no single ARM instruction equivalent for this Thumb
-                   instruction. To keep the simulation simple (from the user
-                   perspective) we check if the following instruction is the
-                   second half of this BL, and if it is we simulate it
-                   immediately.  */
-                valid = t_branch;
-                break;
-        case 31:                /* BL instruction 2 */
-                /* Format 19 */
-                /* There is no single ARM instruction equivalent for this
-                   instruction. Also, it should only ever be matched with the
-                   fmt19 "BL instruction 1" instruction. However, we do allow
-                   the simulation of it on its own, with undefined results if
-                   r14 is not suitably initialised.  */
-                {
-                        #if 0
-                        ARMword tmp = (pc + 2);
-                        state->Reg[15] =
-                                (state->Reg[14] + ((tinstr & 0x07FF) << 1));
-                        state->Reg[14] = (tmp | 1);
-                        #endif
-                        valid = t_branch;
-                }
-                break;
-        }
-        *inst_size = 2;
-        return valid;
+    return valid;
 }
diff --git a/src/core/arm/interpreter/arm_interpreter.cpp b/src/core/arm/interpreter/arm_interpreter.cpp
index e2aa5ce92..80ebc359e 100644
--- a/src/core/arm/interpreter/arm_interpreter.cpp
+++ b/src/core/arm/interpreter/arm_interpreter.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "core/arm/interpreter/arm_interpreter.h"
@@ -38,78 +38,43 @@ ARM_Interpreter::~ARM_Interpreter() {
     delete state;
 }
 
-/**
- * Set the Program Counter to an address
- * @param addr Address to set PC to
- */
 void ARM_Interpreter::SetPC(u32 pc) {
     state->pc = state->Reg[15] = pc;
 }
 
-/*
- * Get the current Program Counter
- * @return Returns current PC
- */
 u32 ARM_Interpreter::GetPC() const {
     return state->pc;
 }
 
-/**
- * Get an ARM register
- * @param index Register index (0-15)
- * @return Returns the value in the register
- */
 u32 ARM_Interpreter::GetReg(int index) const {
     return state->Reg[index];
 }
 
-/**
- * Set an ARM register
- * @param index Register index (0-15)
- * @param value Value to set register to
- */
 void ARM_Interpreter::SetReg(int index, u32 value) {
     state->Reg[index] = value;
 }
 
-/**
- * Get the current CPSR register
- * @return Returns the value of the CPSR register
- */
 u32 ARM_Interpreter::GetCPSR() const {
     return state->Cpsr;
 }
 
-/**
- * Set the current CPSR register
- * @param cpsr Value to set CPSR to
- */
 void ARM_Interpreter::SetCPSR(u32 cpsr) {
     state->Cpsr = cpsr;
 }
 
-/**
- * Returns the number of clock ticks since the last reset
- * @return Returns number of clock ticks
- */
 u64 ARM_Interpreter::GetTicks() const {
-    return ARMul_Time(state);
+    return state->NumInstrs;
+}
+
+void ARM_Interpreter::AddTicks(u64 ticks) {
+    state->NumInstrs += ticks;
 }
 
-/**
- * Executes the given number of instructions
- * @param num_instructions Number of instructions to executes
- */
 void ARM_Interpreter::ExecuteInstructions(int num_instructions) {
     state->NumInstrsToExecute = num_instructions - 1;
     ARMul_Emulate32(state);
 }
 
-/**
- * Saves the current CPU context
- * @param ctx Thread context to save
- * @todo Do we need to save Reg[15] and NextInstr?
- */
 void ARM_Interpreter::SaveContext(ThreadContext& ctx) {
     memcpy(ctx.cpu_registers, state->Reg, sizeof(ctx.cpu_registers));
     memcpy(ctx.fpu_registers, state->ExtReg, sizeof(ctx.fpu_registers));
@@ -126,11 +91,6 @@ void ARM_Interpreter::SaveContext(ThreadContext& ctx) {
     ctx.mode = state->NextInstr;
 }
 
-/**
- * Loads a CPU context
- * @param ctx Thread context to load
- * @param Do we need to load Reg[15] and NextInstr?
- */
 void ARM_Interpreter::LoadContext(const ThreadContext& ctx) {
     memcpy(state->Reg, ctx.cpu_registers, sizeof(ctx.cpu_registers));
     memcpy(state->ExtReg, ctx.fpu_registers, sizeof(ctx.fpu_registers));
@@ -147,7 +107,6 @@ void ARM_Interpreter::LoadContext(const ThreadContext& ctx) {
     state->NextInstr = ctx.mode;
 }
 
-/// Prepare core for thread reschedule (if needed to correctly handle state)
 void ARM_Interpreter::PrepareReschedule() {
     state->NumInstrsToExecute = 0;
 }
diff --git a/src/core/arm/interpreter/arm_interpreter.h b/src/core/arm/interpreter/arm_interpreter.h
index ed53d997c..019dad5df 100644
--- a/src/core/arm/interpreter/arm_interpreter.h
+++ b/src/core/arm/interpreter/arm_interpreter.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -61,6 +61,12 @@ public:
     u64 GetTicks() const override;
 
     /**
+    * Advance the CPU core by the specified number of ticks (e.g. to simulate CPU execution time)
+    * @param ticks Number of ticks to advance the CPU core
+    */
+    void AddTicks(u64 ticks) override;
+
+    /**
      * Saves the current CPU context
      * @param ctx Thread context to save
      */
diff --git a/src/core/arm/interpreter/armemu.cpp b/src/core/arm/interpreter/armemu.cpp
index 825955ade..b9c2aa6c2 100644
--- a/src/core/arm/interpreter/armemu.cpp
+++ b/src/core/arm/interpreter/armemu.cpp
@@ -1166,7 +1166,7 @@ mainswitch:
                 else if ((((int)BITS(21, 27)) == 0x3e) && ((int)BITS(4, 6) == 0x1)) {
                     //(ARMword)(instr<<(31-(n))) >> ((31-(n))+(m))
                     unsigned msb ,tmp_rn, tmp_rd, dst;
-                    msb = tmp_rd = tmp_rn = dst = 0;
+                    tmp_rd = tmp_rn = dst = 0;
                     Rd = BITS(12, 15);
                     Rn = BITS(0, 3);
                     lsb = BITS(7, 11);
@@ -1356,7 +1356,13 @@ mainswitch:
                     }
                     break;
 
-                case 0x04:      /* SUB reg */
+                case 0x04: /* SUB reg */
+                    // Signifies UMAAL
+                    if (state->is_v6 && BITS(4, 7) == 0x09) {
+                        if (handle_v6_insn(state, instr))
+                            break;
+                    }
+
 #ifdef MODET
                     if (BITS (4, 7) == 0xB) {
                         /* STRH immediate offset, no write-back, down, post indexed.  */
@@ -1664,7 +1670,7 @@ mainswitch:
                             op1 *= op2;
                             //printf("SMLA_INST:BB,op1=0x%x, op2=0x%x. Rn=0x%x\n", op1, op2, Rn);
                             if (AddOverflow(op1, Rn, op1 + Rn))
-                                SETS;
+                                SETQ;
                             state->Reg[BITS (16, 19)] = op1 + Rn;
                             break;
                         }
@@ -1676,7 +1682,7 @@ mainswitch:
                             ARMword result = op1 + op2;
                             if (AddOverflow(op1, op2, result)) {
                                 result = POS (result) ? 0x80000000 : 0x7fffffff;
-                                SETS;
+                                SETQ;
                             }
                             state->Reg[BITS (12, 15)] = result;
                             break;
@@ -1718,7 +1724,7 @@ mainswitch:
                             TAKEABORT;
                     } else if ((BITS (0, 11) == 0) && (LHSReg == 15)) { /* MRS CPSR */
                         UNDEF_MRSPC;
-                        DEST = ECC | EINT | EMODE;
+                        DEST = ARMul_GetCPSR(state);
                     } else {
                         UNDEF_Test;
                     }
@@ -1737,7 +1743,7 @@ mainswitch:
                         //chy 2006-02-15 if in user mode, can not set cpsr 0:23
                         //from p165 of ARMARM book
                         state->Cpsr = GETSPSR (state->Bank);
-                        //ARMul_CPSRAltered (state);
+                        ARMul_CPSRAltered (state);
 #else
                         rhs = DPRegRHS;
                         temp = LHS & rhs;
@@ -1789,7 +1795,7 @@ mainswitch:
                                 ARMword Rn = state->Reg[BITS(12, 15)];
 
                                 if (AddOverflow((ARMword)result, Rn, (ARMword)(result + Rn)))
-                                    SETS;
+                                    SETQ;
                                 result += Rn;
                             }
                             state->Reg[BITS (16, 19)] = (ARMword)result;
@@ -1805,7 +1811,7 @@ mainswitch:
                             if (SubOverflow
                                     (op1, op2, result)) {
                                 result = POS (result) ? 0x80000000 : 0x7fffffff;
-                                SETS;
+                                SETQ;
                             }
 
                             state->Reg[BITS (12, 15)] = result;
@@ -1877,7 +1883,7 @@ mainswitch:
                         /* TEQP reg */
 #ifdef MODE32
                         state->Cpsr = GETSPSR (state->Bank);
-                        //ARMul_CPSRAltered (state);
+                        ARMul_CPSRAltered (state);
 #else
                         rhs = DPRegRHS;
                         temp = LHS ^ rhs;
@@ -1928,13 +1934,13 @@ mainswitch:
 
                             if (AddOverflow
                                     (op2, op2, op2d)) {
-                                SETS;
+                                SETQ;
                                 op2d = POS (op2d) ? 0x80000000 : 0x7fffffff;
                             }
 
                             result = op1 + op2d;
                             if (AddOverflow(op1, op2d, result)) {
-                                SETS;
+                                SETQ;
                                 result = POS (result) ? 0x80000000 : 0x7fffffff;
                             }
 
@@ -1993,7 +1999,7 @@ mainswitch:
                         /* CMPP reg.  */
 #ifdef MODE32
                         state->Cpsr = GETSPSR (state->Bank);
-                        //ARMul_CPSRAltered (state);
+                        ARMul_CPSRAltered (state);
 #else
                         rhs = DPRegRHS;
                         temp = LHS - rhs;
@@ -2047,13 +2053,13 @@ mainswitch:
                             ARMword result;
 
                             if (AddOverflow(op2, op2, op2d)) {
-                                SETS;
+                                SETQ;
                                 op2d = POS (op2d) ? 0x80000000 : 0x7fffffff;
                             }
 
                             result = op1 - op2d;
                             if (SubOverflow(op1, op2d, result)) {
-                                SETS;
+                                SETQ;
                                 result = POS (result) ? 0x80000000 : 0x7fffffff;
                             }
 
@@ -2112,7 +2118,7 @@ mainswitch:
                     if (DESTReg == 15) {
 #ifdef MODE32
                         state->Cpsr = GETSPSR (state->Bank);
-                        //ARMul_CPSRAltered (state);
+                        ARMul_CPSRAltered (state);
 #else
                         rhs = DPRegRHS;
                         temp = LHS + rhs;
@@ -2200,17 +2206,57 @@ mainswitch:
                         Handle_Store_Double (state, instr);
                         break;
                     }
+                    if (BITS(4, 11) == 0xF9) { //strexd
+                        u32 l = LHSReg;
+
+                        bool enter = false;
+
+                        if (state->currentexval == (u32)ARMul_ReadWord(state, state->currentexaddr)&&
+                                state->currentexvald == (u32)ARMul_ReadWord(state, state->currentexaddr + 4))
+                            enter = true;
+
+
+                        //todo bug this and STREXD and LDREXD http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.ddi0360e/CHDGJGGC.html
+
+
+                        if (enter) {
+                            ARMul_StoreWordN(state, LHS, state->Reg[RHSReg]);
+                            ARMul_StoreWordN(state,LHS + 4 , state->Reg[RHSReg + 1]);
+                            state->Reg[DESTReg] = 0;
+                        } else {
+                            state->Reg[DESTReg] = 1;
+                        }
+
+                        break;
+                    }
 #endif
                     dest = DPRegRHS;
                     WRITEDEST (dest);
                     break;
 
-                case 0x1b:      /* MOVS reg */
+                case 0x1B:      /* MOVS reg */
 #ifdef MODET
+                    /* ldrexd ichfly */
+                    if (BITS(0, 11) == 0xF9F) { //strexd
+                        lhs = LHS;
+
+                        state->currentexaddr = lhs;
+                        state->currentexval = (u32)ARMul_ReadWord(state, lhs);
+                        state->currentexvald = (u32)ARMul_ReadWord(state, lhs + 4);
+
+                        state->Reg[DESTReg] = ARMul_LoadWordN(state, lhs);
+                        state->Reg[DESTReg] = ARMul_LoadWordN(state, lhs + 4);
+                        break;
+                    }
+
                     if ((BITS (4, 11) & 0xF9) == 0x9)
                         /* LDR register offset, write-back, up, pre indexed.  */
                         LHPREUPWB ();
                     /* Continue with remaining instruction decoding.  */
+
+
+
+
 #endif
                     dest = DPSRegRHS;
                     WRITESDEST (dest);
@@ -2297,12 +2343,12 @@ mainswitch:
                         if (state->currentexval == (u32)ARMul_LoadHalfWord(state, state->currentexaddr))enter = true;
 
 
-                        ARMul_StoreHalfWord(state, lhs, RHS);
                         //StoreWord(state, lhs, RHS)
                         if (state->Aborted) {
                             TAKEABORT;
                         }
                         if (enter) {
+                            ARMul_StoreHalfWord(state, lhs, RHS);
                             state->Reg[DESTReg] = 0;
                         } else {
                             state->Reg[DESTReg] = 1;
@@ -2520,7 +2566,7 @@ mainswitch:
                         /* TSTP immed.  */
 #ifdef MODE32
                         state->Cpsr = GETSPSR (state->Bank);
-                        //ARMul_CPSRAltered (state);
+                        ARMul_CPSRAltered (state);
 #else
                         temp = LHS & DPImmRHS;
                         SETR15PSR (temp);
@@ -2547,7 +2593,7 @@ mainswitch:
                         /* TEQP immed.  */
 #ifdef MODE32
                         state->Cpsr = GETSPSR (state->Bank);
-                        //ARMul_CPSRAltered (state);
+                        ARMul_CPSRAltered (state);
 #else
                         temp = LHS ^ DPImmRHS;
                         SETR15PSR (temp);
@@ -2568,7 +2614,7 @@ mainswitch:
                         /* CMPP immed.  */
 #ifdef MODE32
                         state->Cpsr = GETSPSR (state->Bank);
-                        //ARMul_CPSRAltered (state);
+                        ARMul_CPSRAltered (state);
 #else
                         temp = LHS - DPImmRHS;
                         SETR15PSR (temp);
@@ -2604,7 +2650,7 @@ mainswitch:
                         /* CMNP immed.  */
 #ifdef MODE32
                         state->Cpsr = GETSPSR (state->Bank);
-                        //ARMul_CPSRAltered (state);
+                        ARMul_CPSRAltered (state);
 #else
                         temp = LHS + DPImmRHS;
                         SETR15PSR (temp);
@@ -3054,27 +3100,21 @@ mainswitch:
                     break;
 
                 case 0x68:      /* Store Word, No WriteBack, Post Inc, Reg.  */
-                    //ichfly PKHBT PKHTB todo check this
-                    if ((instr & 0x70) == 0x10) //pkhbt
-                    {
+                    if ((instr & 0x70) == 0x10) { //pkhbt
                         u8 idest = BITS(12, 15);
                         u8 rfis = BITS(16, 19);
                         u8 rlast = BITS(0, 3);
                         u8 ishi = BITS(7,11);
                         state->Reg[idest] = (state->Reg[rfis] & 0xFFFF) | ((state->Reg[rlast] << ishi) & 0xFFFF0000);
                         break;
-                    }
-                    else if ((instr & 0x70) == 0x50)//pkhtb
-                    {
-                        u8 idest = BITS(12, 15);
-                        u8 rfis = BITS(16, 19);
-                        u8 rlast = BITS(0, 3);
-                        u8 ishi = BITS(7, 11);
-                        if (ishi == 0)ishi = 0x20;
-                        state->Reg[idest] = (((int)(state->Reg[rlast]) >> (int)(ishi))& 0xFFFF) | ((state->Reg[rfis]) & 0xFFFF0000);
+                    } else if ((instr & 0x70) == 0x50) { //pkhtb
+                        u8 rd_idx = BITS(12, 15);
+                        u8 rn_idx = BITS(16, 19);
+                        u8 rm_idx = BITS(0, 3);
+                        u8 imm5 = BITS(7, 11) ? BITS(7, 11) : 31;
+                        state->Reg[rd_idx] = ((static_cast<s32>(state->Reg[rm_idx]) >> imm5) & 0xFFFF) | ((state->Reg[rn_idx]) & 0xFFFF0000);
                         break;
-                    }
-                    else if (BIT (4)) {
+                    } else if (BIT (4)) {
 #ifdef MODE32
                         if (state->is_v6
                                 && handle_v6_insn (state, instr))
@@ -3686,13 +3726,11 @@ mainswitch:
 
                 /* Co-Processor Data Transfers.  */
                 case 0xc4:
-                    if ((instr & 0x0FF00FF0) == 0xC400B10) //vmov BIT(0-3), BIT(12-15), BIT(16-20),  vmov d0, r0, r0
-                    {
+                    if ((instr & 0x0FF00FF0) == 0xC400B10) { //vmov BIT(0-3), BIT(12-15), BIT(16-20),  vmov d0, r0, r0
                         state->ExtReg[BITS(0, 3) << 1] = state->Reg[BITS(12, 15)];
                         state->ExtReg[(BITS(0, 3) << 1) + 1] = state->Reg[BITS(16, 20)];
                         break;
-                    }
-                    else if (state->is_v5) {
+                    } else if (state->is_v5) {
                         /* Reading from R15 is UNPREDICTABLE.  */
                         if (BITS (12, 15) == 15 || BITS (16, 19) == 15)
                             ARMul_UndefInstr (state, instr);
@@ -3712,22 +3750,18 @@ mainswitch:
                     break;
 
                 case 0xc5:
-                    if ((instr & 0x00000FF0) == 0xB10) //vmov BIT(12-15), BIT(16-20), BIT(0-3) vmov r0, r0, d0
-                    {
+                    if ((instr & 0x00000FF0) == 0xB10) { //vmov BIT(12-15), BIT(16-20), BIT(0-3) vmov r0, r0, d0
                         state->Reg[BITS(12, 15)] = state->ExtReg[BITS(0, 3) << 1];
                         state->Reg[BITS(16, 19)] = state->ExtReg[(BITS(0, 3) << 1) + 1];
                         break;
-                    }
-                    else if (state->is_v5) {
+                    } else if (state->is_v5) {
                         /* Writes to R15 are UNPREDICATABLE.  */
                         if (DESTReg == 15 || LHSReg == 15)
                             ARMul_UndefInstr (state, instr);
                         /* Is access to the coprocessor allowed ?  */
-                        else if (!CP_ACCESS_ALLOWED(state, CPNum))
-                        {
+                        else if (!CP_ACCESS_ALLOWED(state, CPNum)) {
                             ARMul_UndefInstr(state, instr);
-                        }
-                        else {
+                        } else {
                             /* MRRC, ARMv5TE and up */
                             ARMul_MRRC (state, instr, &DEST, &(state->Reg[LHSReg]));
                             break;
@@ -4565,7 +4599,7 @@ out:
 #ifdef MODE32
         if (state->Bank > 0) {
             state->Cpsr = state->Spsr[state->Bank];
-            //ARMul_CPSRAltered (state);
+            ARMul_CPSRAltered (state);
         }
 #ifdef MODET
         if (TFLAG)
@@ -5256,7 +5290,7 @@ L_ldm_s_makeabort:
             //chy 2006-02-16 , should not consider system mode, don't conside 26bit mode
             if (state->Mode != USER26MODE && state->Mode != USER32MODE ) {
                 state->Cpsr = GETSPSR (state->Bank);
-                //ARMul_CPSRAltered (state);
+                ARMul_CPSRAltered (state);
             }
 
             WriteR15 (state, PC);
@@ -5639,37 +5673,29 @@ L_stm_s_takeabort:
     /* Attempt to emulate an ARMv6 instruction.
        Returns non-zero upon success.  */
 
-    static int
-    handle_v6_insn (ARMul_State * state, ARMword instr) {
-        switch (BITS (20, 27)) {
-        //ichfly
-        case 0x66: //UQSUB8
-            if ((instr & 0x0FF00FF0) == 0x06600FF0) {
-                u32 rd = (instr >> 12) & 0xF;
-                u32 rm = (instr >> 16) & 0xF;
-                u32 rn = (instr >> 0) & 0xF;
-                u32 subfrom = state->Reg[rm];
-                u32 tosub = state->Reg[rn];
-
-                u8 b1 = (u8)((u8)(subfrom)-(u8)(tosub));
-                if (b1 > (u8)(subfrom)) b1 = 0;
-                u8 b2 = (u8)((u8)(subfrom >> 8) - (u8)(tosub >> 8));
-                if (b2 > (u8)(subfrom >> 8)) b2 = 0;
-                u8 b3 = (u8)((u8)(subfrom >> 16) - (u8)(tosub >> 16));
-                if (b3 > (u8)(subfrom >> 16)) b3 = 0;
-                u8 b4 = (u8)((u8)(subfrom >> 24) - (u8)(tosub >> 24));
-                if (b4 > (u8)(subfrom >> 24)) b4 = 0;
-                state->Reg[rd] = (u32)(b1 | b2 << 8 | b3 << 16 | b4 << 24);
-                return 1;
-            } else {
-                printf("UQSUB8 decoding fail %08X",instr);
-            }
-#if 0
+    static int handle_v6_insn(ARMul_State* state, ARMword instr) {
+        switch (BITS(20, 27)) {
         case 0x03:
             printf ("Unhandled v6 insn: ldr\n");
             break;
-        case 0x04:
-            printf ("Unhandled v6 insn: umaal\n");
+        case 0x04: // UMAAL
+            {
+                const u8 rm_idx = BITS(8, 11);
+                const u8 rn_idx = BITS(0, 3);
+                const u8 rd_lo_idx = BITS(12, 15);
+                const u8 rd_hi_idx = BITS(16, 19);
+
+                const u32 rm_val = state->Reg[rm_idx];
+                const u32 rn_val = state->Reg[rn_idx];
+                const u32 rd_lo_val = state->Reg[rd_lo_idx];
+                const u32 rd_hi_val = state->Reg[rd_hi_idx];
+
+                const u64 result = (rn_val * rm_val) + rd_lo_val + rd_hi_val;
+
+                state->Reg[rd_lo_idx] = (result & 0xFFFFFFFF);
+                state->Reg[rd_hi_idx] = ((result >> 32) & 0xFFFFFFFF);
+                return 1;
+            }
             break;
         case 0x06:
             printf ("Unhandled v6 insn: mls/str\n");
@@ -5678,9 +5704,43 @@ L_stm_s_takeabort:
             printf ("Unhandled v6 insn: smi\n");
             break;
         case 0x18:
+                        if (BITS(4, 7) == 0x9) {
+                                /* strex */
+                                u32 l = LHSReg;
+                                u32 r = RHSReg;
+                                u32 lhs = LHS;
+
+                                bool enter = false;
+
+                                if (state->currentexval == (u32)ARMul_ReadWord(state, state->currentexaddr))enter = true;
+                                //StoreWord(state, lhs, RHS)
+                                if (state->Aborted) {
+                                        TAKEABORT;
+                                }
+
+                                if (enter) {
+                                        ARMul_StoreWordS(state, lhs, RHS);
+                                        state->Reg[DESTReg] = 0;
+                                }
+                                else {
+                                        state->Reg[DESTReg] = 1;
+                                }
+
+                                return 1;
+                        }
             printf ("Unhandled v6 insn: strex\n");
             break;
         case 0x19:
+                        /* ldrex */
+                        if (BITS(4, 7) == 0x9) {
+                                u32 lhs = LHS;
+
+                                state->currentexaddr = lhs;
+                                state->currentexval = ARMul_ReadWord(state, lhs);
+
+                                LoadWord(state, instr, lhs);
+                                return 1;
+                        }
             printf ("Unhandled v6 insn: ldrex\n");
             break;
         case 0x1a:
@@ -5690,9 +5750,52 @@ L_stm_s_takeabort:
             printf ("Unhandled v6 insn: ldrexd\n");
             break;
         case 0x1c:
+                        if (BITS(4, 7) == 0x9) {
+                                /* strexb */
+                                u32 lhs = LHS;
+
+                                bool enter = false;
+
+                                if (state->currentexval == (u32)ARMul_ReadByte(state, state->currentexaddr))enter = true;
+
+                                BUSUSEDINCPCN;
+                                if (state->Aborted) {
+                                        TAKEABORT;
+                                }
+
+
+                                if (enter) {
+                                        ARMul_StoreByte(state, lhs, RHS);
+                                        state->Reg[DESTReg] = 0;
+                                }
+                                else {
+                                        state->Reg[DESTReg] = 1;
+                                }
+
+                                //printf("In %s, strexb not implemented\n", __FUNCTION__);
+                                UNDEF_LSRBPC;
+                                /* WRITESDEST (dest); */
+                                return 1;
+                        }
             printf ("Unhandled v6 insn: strexb\n");
             break;
         case 0x1d:
+                        if ((BITS(4, 7)) == 0x9) {
+                                /* ldrexb */
+                                u32 lhs = LHS;
+                                LoadByte(state, instr, lhs, LUNSIGNED);
+
+                                state->currentexaddr = lhs;
+                                state->currentexval = (u32)ARMul_ReadByte(state, lhs);
+
+                                //state->Reg[BITS(12, 15)] = ARMul_LoadByte(state, state->Reg[BITS(16, 19)]);
+                                //printf("ldrexb\n");
+                                //printf("instr is %x rm is %d\n", instr, BITS(16, 19));
+                                //exit(-1);
+
+                                //printf("In %s, ldrexb not implemented\n", __FUNCTION__);
+                                return 1;
+                        }
             printf ("Unhandled v6 insn: ldrexb\n");
             break;
         case 0x1e:
@@ -5713,510 +5816,801 @@ L_stm_s_takeabort:
         case 0x3f:
             printf ("Unhandled v6 insn: rbit\n");
             break;
-#endif
-        case 0x61:
-            if ((instr & 0xFF0) == 0xf70)//ssub16
-            {
-                u8 tar = BITS(12, 15);
-                u8 src1 = BITS(16, 19);
-                u8 src2 = BITS(0, 3);
-                s16 a1 = (state->Reg[src1] & 0xFFFF);
-                s16 a2 = ((state->Reg[src1] >> 0x10) & 0xFFFF);
-                s16 b1 = (state->Reg[src2] & 0xFFFF);
-                s16 b2 = ((state->Reg[src2] >> 0x10) & 0xFFFF);
-                state->Reg[tar] = ((a1 - a2) & 0xFFFF) | (((b1 - b2)&0xFFFF)<< 0x10);
-                return 1;
-            }
-            else if ((instr & 0xFF0) == 0xf10)//sadd16
+        case 0x61: // SADD16, SASX, SSAX, and SSUB16
+            if ((instr & 0xFF0) == 0xf10 || (instr & 0xFF0) == 0xf30 ||
+                (instr & 0xFF0) == 0xf50 || (instr & 0xFF0) == 0xf70)
             {
-                u8 tar = BITS(12, 15);
-                u8 src1 = BITS(16, 19);
-                u8 src2 = BITS(0, 3);
-                s16 a1 = (state->Reg[src1] & 0xFFFF);
-                s16 a2 = ((state->Reg[src1] >> 0x10) & 0xFFFF);
-                s16 b1 = (state->Reg[src2] & 0xFFFF);
-                s16 b2 = ((state->Reg[src2] >> 0x10) & 0xFFFF);
-                state->Reg[tar] = ((a1 + a2) & 0xFFFF) | (((b1 + b2)&0xFFFF)<< 0x10);
+                const u8 rd_idx = BITS(12, 15);
+                const u8 rm_idx = BITS(0, 3);
+                const u8 rn_idx = BITS(16, 19);
+                const s16 rn_lo = (state->Reg[rn_idx] & 0xFFFF);
+                const s16 rn_hi = ((state->Reg[rn_idx] >> 16) & 0xFFFF);
+                const s16 rm_lo = (state->Reg[rm_idx] & 0xFFFF);
+                const s16 rm_hi = ((state->Reg[rm_idx] >> 16) & 0xFFFF);
+
+                s32 lo_result;
+                s32 hi_result;
+
+                // SADD16
+                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);
+                }
+                // SSUB16
+                else {
+                    lo_result = (rn_lo - rm_lo);
+                    hi_result = (rn_hi - rm_hi);
+                }
+
+                state->Reg[rd_idx] = (lo_result & 0xFFFF) | ((hi_result & 0xFFFF) << 16);
+
+                if (lo_result >= 0) {
+                    state->GEFlag |= (1 << 16);
+                    state->GEFlag |= (1 << 17);
+                } else {
+                    state->GEFlag &= ~(1 << 16);
+                    state->GEFlag &= ~(1 << 17);
+                }
+
+                if (hi_result >= 0) {
+                    state->GEFlag |= (1 << 18);
+                    state->GEFlag |= (1 << 19);
+                } else {
+                    state->GEFlag &= ~(1 << 18);
+                    state->GEFlag &= ~(1 << 19);
+                }
+
                 return 1;
             }
-            else if ((instr & 0xFF0) == 0xf50)//ssax
+            // SADD8/SSUB8
+            else  if ((instr & 0xFF0) == 0xf90 || (instr & 0xFF0) == 0xff0)
             {
-                u8 tar = BITS(12, 15);
-                u8 src1 = BITS(16, 19);
-                u8 src2 = BITS(0, 3);
-                s16 a1 = (state->Reg[src1] & 0xFFFF);
-                s16 a2 = ((state->Reg[src1] >> 0x10) & 0xFFFF);
-                s16 b1 = (state->Reg[src2] & 0xFFFF);
-                s16 b2 = ((state->Reg[src2] >> 0x10) & 0xFFFF);
-                state->Reg[tar] = ((a1 + b2) & 0xFFFF) | (((a2 - b1) & 0xFFFF) << 0x10);
+                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->GEFlag |= (1 << 16);
+                    else
+                        state->GEFlag &= ~(1 << 16);
+
+                    if (lo_val2 & 0x80)
+                        state->GEFlag |= (1 << 17);
+                    else
+                        state->GEFlag &= ~(1 << 17);
+
+                    if (hi_val1 & 0x80)
+                        state->GEFlag |= (1 << 18);
+                    else
+                        state->GEFlag &= ~(1 << 18);
+
+                    if (hi_val2 & 0x80)
+                        state->GEFlag |= (1 << 19);
+                    else
+                        state->GEFlag &= ~(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->GEFlag |= (1 << 16);
+                    else
+                        state->GEFlag &= ~(1 << 16);
+
+                    if (!(lo_val2 & 0x80))
+                        state->GEFlag |= (1 << 17);
+                    else
+                        state->GEFlag &= ~(1 << 17);
+
+                    if (!(hi_val1 & 0x80))
+                        state->GEFlag |= (1 << 18);
+                    else
+                        state->GEFlag &= ~(1 << 18);
+
+                    if (!(hi_val2 & 0x80))
+                        state->GEFlag |= (1 << 19);
+                    else
+                        state->GEFlag &= ~(1 << 19);
+                }
+
+                state->Reg[rd_idx] = (lo_val1 | lo_val2 << 8 | hi_val1 << 16 | hi_val2 << 24);
                 return 1;
             }
-            else if ((instr & 0xFF0) == 0xf30)//sasx
-            {
-                u8 tar = BITS(12, 15);
-                u8 src1 = BITS(16, 19);
-                u8 src2 = BITS(0, 3);
-                s16 a1 = (state->Reg[src1] & 0xFFFF);
-                s16 a2 = ((state->Reg[src1] >> 0x10) & 0xFFFF);
-                s16 b1 = (state->Reg[src2] & 0xFFFF);
-                s16 b2 = ((state->Reg[src2] >> 0x10) & 0xFFFF);
-                state->Reg[tar] = ((a1 - b2) & 0xFFFF) | (((a2 + b1) & 0xFFFF) << 0x10);
-                return 1;
+            else {
+                printf("Unhandled v6 insn: %08x", instr);
             }
-            else printf ("Unhandled v6 insn: sadd/ssub\n");
             break;
-        case 0x62:
-            if ((instr & 0xFF0) == 0xf70)//QSUB16
+        case 0x62: // QADD16, QASX, QSAX, QSUB16, QADD8, and QSUB8
             {
-                u8 tar = BITS(12, 15);
-                u8 src1 = BITS(16, 19);
-                u8 src2 = BITS(0, 3);
-                s16 a1 = (state->Reg[src1] & 0xFFFF);
-                s16 a2 = ((state->Reg[src1] >> 0x10) & 0xFFFF);
-                s16 b1 = (state->Reg[src2] & 0xFFFF);
-                s16 b2 = ((state->Reg[src2] >> 0x10) & 0xFFFF);
-                s32 res1 = (a1 - b1);
-                s32 res2 = (a2 - b2);
-                if (res1 > 0x7FFF) res1 = 0x7FFF;
-                if (res2 > 0x7FFF) res2 = 0x7FFF;
-                if (res1 < 0x7FFF) res1 = -0x8000;
-                if (res2 < 0x7FFF) res2 = -0x8000;
-                state->Reg[tar] = (res1 & 0xFFFF) | ((res2 & 0xFFFF) << 0x10);
-                return 1;
-            }
-            else if ((instr & 0xFF0) == 0xf10)//QADD16
-            {
-                u8 tar = BITS(12, 15);
-                u8 src1 = BITS(16, 19);
-                u8 src2 = BITS(0, 3);
-                s16 a1 = (state->Reg[src1] & 0xFFFF);
-                s16 a2 = ((state->Reg[src1] >> 0x10) & 0xFFFF);
-                s16 b1 = (state->Reg[src2] & 0xFFFF);
-                s16 b2 = ((state->Reg[src2] >> 0x10) & 0xFFFF);
-                s32 res1 = (a1 + b1);
-                s32 res2 = (a2 + b2);
-                if (res1 > 0x7FFF) res1 = 0x7FFF;
-                if (res2 > 0x7FFF) res2 = 0x7FFF;
-                if (res1 < 0x7FFF) res1 = -0x8000;
-                if (res2 < 0x7FFF) res2 = -0x8000;
-                state->Reg[tar] = ((res1) & 0xFFFF) | (((res2) & 0xFFFF) << 0x10);
+                const u8 op2 = BITS(5, 7);
+
+                const u8 rd_idx = BITS(12, 15);
+                const u8 rn_idx = BITS(16, 19);
+                const u8 rm_idx = BITS(0, 3);
+                const u16 rm_lo = (state->Reg[rm_idx] & 0xFFFF);
+                const u16 rm_hi = ((state->Reg[rm_idx] >> 0x10) & 0xFFFF);
+                const u16 rn_lo = (state->Reg[rn_idx] & 0xFFFF);
+                const u16 rn_hi = ((state->Reg[rn_idx] >> 0x10) & 0xFFFF);
+
+                u16 lo_result = 0;
+                u16 hi_result = 0;
+
+                // QADD16
+                if (op2 == 0x00) {
+                    lo_result = ARMul_SignedSaturatedAdd16(rn_lo, rm_lo);
+                    hi_result = ARMul_SignedSaturatedAdd16(rn_hi, rm_hi);
+                }
+                // QASX
+                else if (op2 == 0x01) {
+                    lo_result = ARMul_SignedSaturatedSub16(rn_lo, rm_hi);
+                    hi_result = ARMul_SignedSaturatedAdd16(rn_hi, rm_lo);
+                }
+                // QSAX
+                else if (op2 == 0x02) {
+                    lo_result = ARMul_SignedSaturatedAdd16(rn_lo, rm_hi);
+                    hi_result = ARMul_SignedSaturatedSub16(rn_hi, rm_lo);
+                }
+                // QSUB16
+                else if (op2 == 0x03) {
+                    lo_result = ARMul_SignedSaturatedSub16(rn_lo, rm_lo);
+                    hi_result = ARMul_SignedSaturatedSub16(rn_hi, rm_hi);
+                }
+                // QADD8
+                else if (op2 == 0x04) {
+                    lo_result = ARMul_SignedSaturatedAdd8(rn_lo & 0xFF, rm_lo & 0xFF) |
+                                ARMul_SignedSaturatedAdd8(rn_lo >> 8, rm_lo >> 8) << 8;
+                    hi_result = ARMul_SignedSaturatedAdd8(rn_hi & 0xFF, rm_hi & 0xFF) |
+                                ARMul_SignedSaturatedAdd8(rn_hi >> 8, rm_hi >> 8) << 8;
+                }
+                // QSUB8
+                else if (op2 == 0x07) {
+                    lo_result = ARMul_SignedSaturatedSub8(rn_lo & 0xFF, rm_lo & 0xFF) |
+                                ARMul_SignedSaturatedSub8(rn_lo >> 8, rm_lo >> 8) << 8;
+                    hi_result = ARMul_SignedSaturatedSub8(rn_hi & 0xFF, rm_hi & 0xFF) |
+                                ARMul_SignedSaturatedSub8(rn_hi >> 8, rm_hi >> 8) << 8;
+                }
+
+                state->Reg[rd_idx] = (lo_result & 0xFFFF) | ((hi_result & 0xFFFF) << 16);
                 return 1;
             }
-            else printf ("Unhandled v6 insn: qadd/qsub\n");
             break;
-#if 0
         case 0x63:
             printf ("Unhandled v6 insn: shadd/shsub\n");
             break;
         case 0x65:
-            printf ("Unhandled v6 insn: uadd/usub\n");
-            break;
-        case 0x66:
-            printf ("Unhandled v6 insn: uqadd/uqsub\n");
-            break;
-        case 0x67:
-            printf ("Unhandled v6 insn: uhadd/uhsub\n");
-            break;
-        case 0x68:
-            printf ("Unhandled v6 insn: pkh/sxtab/selsxtb\n");
-            break;
-#endif
-        case 0x6c:
-            if ((instr & 0xf03f0) == 0xf0070) //uxtb16
-            {
-                u8 src1 = BITS(0, 3);
-                u8 tar = BITS(12, 15);
-                u32 base = state->Reg[src1];
-                u32 shamt = BITS(9,10)* 8;
-                u32 in = ((base << (32 - shamt)) | (base >> shamt));
-                state->Reg[tar] = in & 0x00FF00FF;
-                return 1;
-            }
-            else
-                printf ("Unhandled v6 insn: uxtb16/uxtab16\n");
-            break;
-        case 0x70:
-            if ((instr & 0xf0d0) == 0xf010)//smuad //ichfly
-            {
-                u8 tar = BITS(16, 19);
-                u8 src1 = BITS(0, 3);
-                u8 src2 = BITS(8, 11);
-                u8 swap = BIT(5);
-                s16 a1 = (state->Reg[src1] & 0xFFFF);
-                s16 a2 = ((state->Reg[src1] >> 0x10) & 0xFFFF);
-                s16 b1 = swap ? ((state->Reg[src2] >> 0x10) & 0xFFFF) : (state->Reg[src2] & 0xFFFF);
-                s16 b2 = swap ? (state->Reg[src2] & 0xFFFF) : ((state->Reg[src2] >> 0x10) & 0xFFFF);
-                state->Reg[tar] = a1*a2 + b1*b2;
-                return 1;
+        {
+            u32 rd = (instr >> 12) & 0xF;
+            u32 rn = (instr >> 16) & 0xF;
+            u32 rm = (instr >> 0) & 0xF;
+            u32 from = state->Reg[rn];
+            u32 to = state->Reg[rm];
+
+            if ((instr & 0xFF0) == 0xF10 || (instr & 0xFF0) == 0xF70) { // UADD16/USUB16
+                u32 h1, h2;
+                state->Cpsr &= 0xfff0ffff;
+                if ((instr & 0x0F0) == 0x070) { // USUB16
+                    h1 = ((u16)from - (u16)to);
+                    h2 = ((u16)(from >> 16) - (u16)(to >> 16));
+
+                    if (!(h1 & 0xffff0000))
+                        state->GEFlag |= (3 << 16);
+                    else
+                        state->GEFlag &= ~(3 << 16);
 
-            }
-            else if ((instr & 0xf0d0) == 0xf050)//smusd
-            {
-                u8 tar = BITS(16, 19);
-                u8 src1 = BITS(0, 3);
-                u8 src2 = BITS(8, 11);
-                u8 swap = BIT(5);
-                s16 a1 = (state->Reg[src1] & 0xFFFF);
-                s16 a2 = ((state->Reg[src1] >> 0x10) & 0xFFFF);
-                s16 b1 = swap ? ((state->Reg[src2] >> 0x10) & 0xFFFF) : (state->Reg[src2] & 0xFFFF);
-                s16 b2 = swap ? (state->Reg[src2] & 0xFFFF) : ((state->Reg[src2] >> 0x10) & 0xFFFF);
-                state->Reg[tar] = a1*a2 - b1*b2;
-                return 1;
-            }
-            else if ((instr & 0xd0) == 0x10)//smlad
-            {
-                u8 tar = BITS(16, 19);
-                u8 src1 = BITS(0, 3);
-                u8 src2 = BITS(8, 11);
-                u8 src3 = BITS(12, 15);
-                u8 swap = BIT(5);
-
-                u32 a3 = state->Reg[src3];
-
-                s16 a1 = (state->Reg[src1] & 0xFFFF);
-                s16 a2 = ((state->Reg[src1] >> 0x10) & 0xFFFF);
-                s16 b1 = swap ? ((state->Reg[src2] >> 0x10) & 0xFFFF) : (state->Reg[src2] & 0xFFFF);
-                s16 b2 = swap ? (state->Reg[src2] & 0xFFFF) : ((state->Reg[src2] >> 0x10) & 0xFFFF);
-                state->Reg[tar] = a1*a2 + b1*b2 + a3;
+                    if (!(h2 & 0xffff0000))
+                        state->GEFlag |= (3 << 18);
+                    else
+                        state->GEFlag &= ~(3 << 18);
+                }
+                else { // UADD16
+                    h1 = ((u16)from + (u16)to);
+                    h2 = ((u16)(from >> 16) + (u16)(to >> 16));
+
+                    if (h1 & 0xffff0000)
+                        state->GEFlag |= (3 << 16);
+                    else
+                        state->GEFlag &= ~(3 << 16);
+
+                    if (h2 & 0xffff0000)
+                        state->GEFlag |= (3 << 18);
+                    else
+                        state->GEFlag &= ~(3 << 18);
+                }
+
+                state->Reg[rd] = (u32)((h1 & 0xffff) | ((h2 & 0xffff) << 16));
                 return 1;
             }
-            else printf ("Unhandled v6 insn: smuad/smusd/smlad/smlsd\n");
-            break;
-        case 0x74:
-            printf ("Unhandled v6 insn: smlald/smlsld\n");
-            break;
-        case 0x75:
-            printf ("Unhandled v6 insn: smmla/smmls/smmul\n");
-            break;
-        case 0x78:
-            printf ("Unhandled v6 insn: usad/usada8\n");
-            break;
-#if 0
-        case 0x7a:
-            printf ("Unhandled v6 insn: usbfx\n");
-            break;
-        case 0x7c:
-            printf ("Unhandled v6 insn: bfc/bfi\n");
-            break;
-#endif
+            else
+                if ((instr & 0xFF0) == 0xF90 || (instr & 0xFF0) == 0xFF0) { // UADD8/USUB8
+                    u32 b1, b2, b3, b4;
+                    state->Cpsr &= 0xfff0ffff;
+                    if ((instr & 0x0F0) == 0x0F0) { // USUB8
+                        b1 = ((u8)from - (u8)to);
+                        b2 = ((u8)(from >> 8) - (u8)(to >> 8));
+                        b3 = ((u8)(from >> 16) - (u8)(to >> 16));
+                        b4 = ((u8)(from >> 24) - (u8)(to >> 24));
+
+                        if (!(b1 & 0xffffff00))
+                            state->GEFlag |= (1 << 16);
+                        else
+                            state->GEFlag &= ~(1 << 16);
 
+                        if (!(b2 & 0xffffff00))
+                            state->GEFlag |= (1 << 17);
+                        else
+                            state->GEFlag &= ~(1 << 17);
 
-            /* add new instr for arm v6. */
-            ARMword lhs, temp;
-        case 0x18: {    /* ORR reg */
-            /* dyf add armv6 instr strex  2010.9.17 */
-            if (BITS (4, 7) == 0x9) {
-                u32 l = LHSReg;
-                u32 r = RHSReg;
-                lhs = LHS;
+                        if (!(b3 & 0xffffff00))
+                            state->GEFlag |= (1 << 18);
+                        else
+                            state->GEFlag &= ~(1 << 18);
 
-                bool enter = false;
+                        if (!(b4 & 0xffffff00))
+                            state->GEFlag |= (1 << 19);
+                        else
+                            state->GEFlag &= ~(1 << 19);
+                    }
+                    else { // UADD8
+                        b1 = ((u8)from + (u8)to);
+                        b2 = ((u8)(from >> 8) + (u8)(to >> 8));
+                        b3 = ((u8)(from >> 16) + (u8)(to >> 16));
+                        b4 = ((u8)(from >> 24) + (u8)(to >> 24));
 
-                if (state->currentexval == (u32)ARMul_ReadWord(state, state->currentexaddr))enter = true;
-                ARMul_StoreWordS(state, lhs, RHS);
-                //StoreWord(state, lhs, RHS)
-                if (state->Aborted) {
-                    TAKEABORT;
-                }
+                        if (b1 & 0xffffff00)
+                            state->GEFlag |= (1 << 16);
+                        else
+                            state->GEFlag &= ~(1 << 16);
 
-                if (enter) {
-                    state->Reg[DESTReg] = 0;
-                } else {
-                    state->Reg[DESTReg] = 1;
-                }
+                        if (b2 & 0xffffff00)
+                            state->GEFlag |= (1 << 17);
+                        else
+                            state->GEFlag &= ~(1 << 17);
 
-                return 1;
-            }
-            break;
-        }
+                        if (b3 & 0xffffff00)
+                            state->GEFlag |= (1 << 18);
+                        else
+                            state->GEFlag &= ~(1 << 18);
 
-        case 0x19: {    /* orrs reg */
-            /* dyf add armv6 instr ldrex  */
-            if (BITS (4, 7) == 0x9) {
-                lhs = LHS;
+                        if (b4 & 0xffffff00)
+                            state->GEFlag |= (1 << 19);
+                        else
+                            state->GEFlag &= ~(1 << 19);
+                    }
 
-                state->currentexaddr = lhs;
-                state->currentexval = ARMul_ReadWord(state, lhs);
+                    state->Reg[rd] = (u32)(b1 | (b2 & 0xff) << 8 | (b3 & 0xff) << 16 | (b4 & 0xff) << 24);
+                    return 1;
+                }
+        }
+            printf("Unhandled v6 insn: uasx/usax\n");
+            break;
+        case 0x66: // UQADD16, UQASX, UQSAX, UQSUB16, UQADD8, and UQSUB8
+            {
+                const u8 rd_idx = BITS(12, 15);
+                const u8 rm_idx = BITS(0, 3);
+                const u8 rn_idx = BITS(16, 19);
+                const u8 op2    = BITS(5, 7);
+                const u32 rm_val = state->Reg[rm_idx];
+                const u32 rn_val = state->Reg[rn_idx];
+
+                u16 lo_val = 0;
+                u16 hi_val = 0;
+
+                // UQADD16
+                if (op2 == 0x00) {
+                    lo_val = ARMul_UnsignedSaturatedAdd16(rn_val & 0xFFFF, rm_val & 0xFFFF);
+                    hi_val = ARMul_UnsignedSaturatedAdd16((rn_val >> 16) & 0xFFFF, (rm_val >> 16) & 0xFFFF);
+                }
+                // UQASX
+                else if (op2 == 0x01) {
+                    lo_val = ARMul_UnsignedSaturatedSub16(rn_val & 0xFFFF, (rm_val >> 16) & 0xFFFF);
+                    hi_val = ARMul_UnsignedSaturatedAdd16((rn_val >> 16) & 0xFFFF, rm_val & 0xFFFF);
+                }
+                // UQSAX
+                else if (op2 == 0x02) {
+                    lo_val = ARMul_UnsignedSaturatedAdd16(rn_val & 0xFFFF, (rm_val >> 16) & 0xFFFF);
+                    hi_val = ARMul_UnsignedSaturatedSub16((rn_val >> 16) & 0xFFFF, rm_val & 0xFFFF);
+                }
+                // UQSUB16
+                else if (op2 == 0x03) {
+                    lo_val = ARMul_UnsignedSaturatedSub16(rn_val & 0xFFFF, rm_val & 0xFFFF);
+                    hi_val = ARMul_UnsignedSaturatedSub16((rn_val >> 16) & 0xFFFF, (rm_val >> 16) & 0xFFFF);
+                }
+                // UQADD8
+                else if (op2 == 0x04) {
+                    lo_val = ARMul_UnsignedSaturatedAdd8(rn_val, rm_val) |
+                             ARMul_UnsignedSaturatedAdd8(rn_val >> 8,  rm_val >> 8) << 8;
+                    hi_val = ARMul_UnsignedSaturatedAdd8(rn_val >> 16, rm_val >> 16) |
+                             ARMul_UnsignedSaturatedAdd8(rn_val >> 24, rm_val >> 24) << 8;
+                }
+                // UQSUB8
+                else {
+                    lo_val = ARMul_UnsignedSaturatedSub8(rn_val, rm_val) |
+                             ARMul_UnsignedSaturatedSub8(rn_val >> 8,  rm_val >> 8) << 8;
+                    hi_val = ARMul_UnsignedSaturatedSub8(rn_val >> 16, rm_val >> 16) |
+                             ARMul_UnsignedSaturatedSub8(rn_val >> 24, rm_val >> 24) << 8;
+                }
 
-                LoadWord (state, instr, lhs);
+                state->Reg[rd_idx] = ((lo_val & 0xFFFF) | hi_val << 16);
                 return 1;
             }
             break;
-        }
+        case 0x67: // UHADD16, UHASX, UHSAX, UHSUB16, UHADD8, and UHSUB8.
+            {
+                const u8 op2 = BITS(5, 7);
 
-        case 0x1c: {    /* BIC reg */
-            /* dyf add for STREXB */
-            if (BITS (4, 7) == 0x9) {
-                lhs = LHS;
+                const u8 rm_idx = BITS(0, 3);
+                const u8 rn_idx = BITS(16, 19);
+                const u8 rd_idx = BITS(12, 15);
 
-                bool enter = false;
+                const u32 rm_val = state->Reg[rm_idx];
+                const u32 rn_val = state->Reg[rn_idx];
 
-                if (state->currentexval == (u32)ARMul_ReadByte(state, state->currentexaddr))enter = true;
+                if (op2 == 0x00 || op2 == 0x01 || op2 == 0x02 || op2 == 0x03)
+                {
+                    u32 lo_val = 0;
+                    u32 hi_val = 0;
 
-                ARMul_StoreByte (state, lhs, RHS);
-                BUSUSEDINCPCN;
-                if (state->Aborted) {
-                    TAKEABORT;
-                }
+                    // UHADD16
+                    if (op2 == 0x00) {
+                        lo_val = (rn_val & 0xFFFF) + (rm_val & 0xFFFF);
+                        hi_val = ((rn_val >> 16) & 0xFFFF) + ((rm_val >> 16) & 0xFFFF);
+                    }
+                    // UHASX
+                    else if (op2 == 0x01) {
+                        lo_val = (rn_val & 0xFFFF) - ((rm_val >> 16) & 0xFFFF);
+                        hi_val = ((rn_val >> 16) & 0xFFFF) + (rm_val & 0xFFFF);
+                    }
+                    // UHSAX
+                    else if (op2 == 0x02) {
+                        lo_val = (rn_val & 0xFFFF) + ((rm_val >> 16) & 0xFFFF);
+                        hi_val = ((rn_val >> 16) & 0xFFFF) - (rm_val & 0xFFFF);
+                    }
+                    // UHSUB16
+                    else if (op2 == 0x03) {
+                        lo_val = (rn_val & 0xFFFF) - (rm_val & 0xFFFF);
+                        hi_val = ((rn_val >> 16) & 0xFFFF) - ((rm_val >> 16) & 0xFFFF);
+                    }
 
+                    lo_val >>= 1;
+                    hi_val >>= 1;
 
-                if (enter) {
-                    state->Reg[DESTReg] = 0;
-                } else {
-                    state->Reg[DESTReg] = 1;
+                    state->Reg[rd_idx] = (lo_val & 0xFFFF) | ((hi_val & 0xFFFF) << 16);
+                    return 1;
+                }
+                else if (op2 == 0x04 || op2 == 0x07) {
+                    u32 sum1;
+                    u32 sum2;
+                    u32 sum3;
+                    u32 sum4;
+
+                    // UHADD8
+                    if (op2 == 0x04) {
+                        sum1 = (rn_val & 0xFF) + (rm_val & 0xFF);
+                        sum2 = ((rn_val >> 8) & 0xFF) + ((rm_val >> 8) & 0xFF);
+                        sum3 = ((rn_val >> 16) & 0xFF) + ((rm_val >> 16) & 0xFF);
+                        sum4 = ((rn_val >> 24) & 0xFF) + ((rm_val >> 24) & 0xFF);
+                    }
+                    // UHSUB8
+                    else {
+                        sum1 = (rn_val & 0xFF) - (rm_val & 0xFF);
+                        sum2 = ((rn_val >> 8) & 0xFF) - ((rm_val >> 8) & 0xFF);
+                        sum3 = ((rn_val >> 16) & 0xFF) - ((rm_val >> 16) & 0xFF);
+                        sum4 = ((rn_val >> 24) & 0xFF) - ((rm_val >> 24) & 0xFF);
+                    }
+
+                    sum1 >>= 1;
+                    sum2 >>= 1;
+                    sum3 >>= 1;
+                    sum4 >>= 1;
+
+                    state->Reg[rd_idx] = (sum1 & 0xFF) | ((sum2 & 0xFF) << 8) | ((sum3 & 0xFF) << 16) | ((sum4 & 0xFF) << 24);
+                    return 1;
                 }
-
-                //printf("In %s, strexb not implemented\n", __FUNCTION__);
-                UNDEF_LSRBPC;
-                /* WRITESDEST (dest); */
-                return 1;
             }
             break;
+        case 0x68:
+        {
+            u32 rd = (instr >> 12) & 0xF;
+            u32 rn = (instr >> 16) & 0xF;
+            u32 rm = (instr >> 0) & 0xF;
+            u32 from = state->Reg[rn];
+            u32 to = state->Reg[rm];
+            u32 cpsr = ARMul_GetCPSR(state);
+            if ((instr & 0xFF0) == 0xFB0) { // SEL
+                u32 result;
+                if (cpsr & (1 << 16))
+                    result = from & 0xff;
+                else
+                    result = to & 0xff;
+                if (cpsr & (1 << 17))
+                    result |= from & 0x0000ff00;
+                else
+                    result |= to & 0x0000ff00;
+                if (cpsr & (1 << 18))
+                    result |= from & 0x00ff0000;
+                else
+                    result |= to & 0x00ff0000;
+                if (cpsr & (1 << 19))
+                    result |= from & 0xff000000;
+                else
+                    result |= to & 0xff000000;
+                state->Reg[rd] = result;
+                return 1;
+            }
         }
+        printf("Unhandled v6 insn: pkh/sxtab/selsxtb\n");
+        break;
+
+                case 0x6a: // SSAT, SSAT16, SXTB, and SXTAB
+                {
+                        const u8 op2 = BITS(5, 7);
+
+                        // SSAT16
+                        if (op2 == 0x01) {
+                                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;
+                                        SETQ;
+                                } else if (rn_lo < min) {
+                                        rn_lo = min;
+                                        SETQ;
+                                }
+
+                                if (rn_hi > max) {
+                                        rn_hi = max;
+                                        SETQ;
+                                } else if (rn_hi < min) {
+                                        rn_hi = min;
+                                        SETQ;
+                                }
+
+                                state->Reg[rd_idx] = (rn_lo & 0xFFFF) | ((rn_hi & 0xFFFF) << 16);
+                                return 1;
+                        }
+                        else if (op2 == 0x03) {
+                                const u8 rotation = BITS(10, 11) * 8;
+                                u32 rm = ((state->Reg[BITS(0, 3)] >> rotation) & 0xFF) | (((state->Reg[BITS(0, 3)] << (32 - rotation)) & 0xFF) & 0xFF);
+                                if (rm & 0x80)
+                                        rm |= 0xffffff00;
+        
+                                // SXTB, otherwise SXTAB
+                                if (BITS(16, 19) == 0xf)
+                                        state->Reg[BITS(12, 15)] = rm;
+                                else
+                                        state->Reg[BITS(12, 15)] = state->Reg[BITS(16, 19)] + rm;
+
+                                return 1;
+                        }
+                        else {
+                                printf("Unimplemented op: SSAT");
+                        }
+                }
+                break;
+
+                case 0x6b: // REV, REV16, SXTH, and SXTAH
+                {
+                        const u8 op2 = BITS(5, 7);
+
+                        // REV
+                        if (op2 == 0x01) {
+                                DEST = ((RHS & 0xFF) << 24) | ((RHS & 0xFF00)) << 8 | ((RHS & 0xFF0000) >> 8) | ((RHS & 0xFF000000) >> 24);
+                                return 1;
+                        }
+                        // REV16
+                        else if (op2 == 0x05) {
+                                DEST = ((RHS & 0xFF) << 8) | ((RHS & 0xFF00)) >> 8 | ((RHS & 0xFF0000) << 8) | ((RHS & 0xFF000000) >> 8);
+                                return 1;
+                        }
+                        else if (op2 == 0x03) {
+                                const u8 rotate = BITS(10, 11) * 8;
+
+                                u32 rm = ((state->Reg[BITS(0, 3)] >> rotate) & 0xFFFF) | (((state->Reg[BITS(0, 3)] << (32 - rotate)) & 0xFFFF) & 0xFFFF);
+                                if (rm & 0x8000)
+                                        rm |= 0xffff0000;
+
+                                // SXTH, otherwise SXTAH
+                                if (BITS(16, 19) == 15)
+                                        state->Reg[BITS(12, 15)] = rm;
+                                else
+                                        state->Reg[BITS(12, 15)] = state->Reg[BITS(16, 19)] + rm;
+
+                                return 1;
+                        }
+                }
+                break;
+
+                case 0x6c: // UXTB16 and UXTAB16
+                        {
+                                const u8 rm_idx = BITS(0, 3);
+                                const u8 rn_idx = BITS(16, 19);
+                                const u8 rd_idx = BITS(12, 15);
+                                const u32 rm_val = state->Reg[rm_idx];
+                                const u32 rn_val = state->Reg[rn_idx];
+                                const u32 rotation = BITS(10, 11) * 8;
+                                const u32 rotated_rm = ((rm_val << (32 - rotation)) | (rm_val >> rotation));
+
+                                // UXTB16
+                                if ((instr & 0xf03f0) == 0xf0070) {
+                                        state->Reg[rd_idx] = rotated_rm & 0x00FF00FF;
+                                }
+                                else { // UXTAB16
+                                        const u8 lo_rotated = (rotated_rm & 0xFF);
+                                        const u16 lo_result = (rn_val & 0xFFFF) + (u16)lo_rotated;
+
+                                        const u8 hi_rotated = (rotated_rm >> 16) & 0xFF;
+                                        const u16 hi_result = (rn_val >> 16) + (u16)hi_rotated;
+
+                                        state->Reg[rd_idx] = ((hi_result << 16) | (lo_result & 0xFFFF));
+                                }
+
+                                return 1;
+                        }
+                        break;
+                case 0x6e: // USAT, USAT16, UXTB, and UXTAB
+                {
+                        const u8 op2 = BITS(5, 7);
+
+                        // USAT16
+                        if (op2 == 0x01) {
+                                const u8 rd_idx = BITS(12, 15);
+                                const u8 rn_idx = BITS(0, 3);
+                                const u8 num_bits = BITS(16, 19);
+                                const s16 max = 0xFFFF >> (16 - num_bits);
+                                s16 rn_lo = (state->Reg[rn_idx]);
+                                s16 rn_hi = (state->Reg[rn_idx] >> 16);
+                                
+                                if (max < rn_lo) {
+                                        rn_lo = max;
+                                        SETQ;
+                                } else if (rn_lo < 0) {
+                                        rn_lo = 0;
+                                        SETQ;
+                                }
+                                
+                                if (max < rn_hi) {
+                                        rn_hi = max;
+                                        SETQ;
+                                } else if (rn_hi < 0) {
+                                        rn_hi = 0;
+                                        SETQ;
+                                }
+                                
+                                state->Reg[rd_idx] = (rn_lo & 0xFFFF) | ((rn_hi << 16) & 0xFFFF);
+                                return 1;
+                        }
+                        else if (op2 == 0x03) {
+                                const u8 rotate = BITS(10, 11) * 8;
+                                const u32 rm = ((state->Reg[BITS(0, 3)] >> rotate) & 0xFF) | (((state->Reg[BITS(0, 3)] << (32 - rotate)) & 0xFF) & 0xFF);
+                                
+                                if (BITS(16, 19) == 0xf)
+                                /* UXTB */
+                                        state->Reg[BITS(12, 15)] = rm;
+                                else
+                                /* UXTAB */
+                                        state->Reg[BITS(12, 15)] = state->Reg[BITS(16, 19)] + rm;
+
+                                return 1;
+                        }
+                        else {
+                                printf("Unimplemented op: USAT");
+                        }
+                }
+                break;
+
+                case 0x6f: // UXTH, UXTAH, and REVSH.
+                {
+                        const u8 op2 = BITS(5, 7);
+
+                        // REVSH
+                        if (op2 == 0x05) {
+                                DEST = ((RHS & 0xFF) << 8) | ((RHS & 0xFF00) >> 8);
+                                if (DEST & 0x8000)
+                                        DEST |= 0xffff0000;
+                                return 1;
+                        }
+                        // UXTH and UXTAH
+                        else if (op2 == 0x03) {
+                                const u8 rotate = BITS(10, 11) * 8;
+                                const ARMword rm = ((state->Reg[BITS(0, 3)] >> rotate) & 0xFFFF) | (((state->Reg[BITS(0, 3)] << (32 - rotate)) & 0xFFFF) & 0xFFFF);
+
+                                // UXTH
+                                if (BITS(16, 19) == 0xf) {
+                                        state->Reg[BITS(12, 15)] = rm;
+                                }
+                                // UXTAH
+                                else {
+                                        state->Reg[BITS(12, 15)] = state->Reg[BITS(16, 19)] + rm;
+                                }
+
+                                return 1;
+                        }
+                }
+        case 0x70:
+            // ichfly
+            // 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);
 
-        case 0x1d: {    /* BICS reg */
-            if ((BITS (4, 7)) == 0x9) {
-                /* ldrexb */
-                temp = LHS;
-                LoadByte (state, instr, temp, LUNSIGNED);
+                u32 rm_val = state->Reg[rm_idx];
+                const u32 rn_val = state->Reg[rn_idx];
 
-                state->currentexaddr = temp;
-                state->currentexval = (u32)ARMul_ReadByte(state, temp);
+                if (do_swap)
+                    rm_val = (((rm_val & 0xFFFF) << 16) | (rm_val >> 16));
 
-                //state->Reg[BITS(12, 15)] = ARMul_LoadByte(state, state->Reg[BITS(16, 19)]);
-                //printf("ldrexb\n");
-                //printf("instr is %x rm is %d\n", instr, BITS(16, 19));
-                //exit(-1);
+                const s16 rm_lo = (rm_val & 0xFFFF);
+                const s16 rm_hi = ((rm_val >> 16) & 0xFFFF);
+                const s16 rn_lo = (rn_val & 0xFFFF);
+                const s16 rn_hi = ((rn_val >> 16) & 0xFFFF);
 
-                //printf("In %s, ldrexb not implemented\n", __FUNCTION__);
-                return 1;
-            }
-            break;
-        }
-        /* add end */
+                const u32 product1 = (rn_lo * rm_lo);
+                const u32 product2 = (rn_hi * rm_hi);
 
-        case 0x6a: {
-            ARMword Rm;
-            int ror = -1;
+                // SMUAD and SMLAD
+                if (BIT(6) == 0) {
+                    state->Reg[rd_idx] = product1 + product2;
 
-            switch (BITS (4, 11)) {
-            case 0x07:
-                ror = 0;
-                break;
-            case 0x47:
-                ror = 8;
-                break;
-            case 0x87:
-                ror = 16;
-                break;
-            case 0xc7:
-                ror = 24;
-                break;
+                    if (BITS(12, 15) != 15) {
+                        state->Reg[rd_idx] += state->Reg[ra_idx];
+                        ARMul_AddOverflowQ(state, product1 + product2, state->Reg[ra_idx]);
+                    }
 
-            case 0x01:
-            case 0xf3:
-                //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);
-            }
+                    ARMul_AddOverflowQ(state, product1, product2);
+                }
+                // SMUSD and SMLSD
+                else {
+                    state->Reg[rd_idx] = product1 - product2;
+                    
+                    if (BITS(12, 15) != 15)
+                        state->Reg[rd_idx] += state->Reg[ra_idx];
+                }
 
                 return 1;
-            default:
-                break;
             }
-
-            if (ror == -1) {
-                if (BITS (4, 6) == 0x7) {
-                    printf ("Unhandled v6 insn: ssat\n");
-                    return 0;
+            break;
+        case 0x74: // SMLALD and SMLSLD
+            {
+                const u8 rm_idx = BITS(8, 11);
+                const u8 rn_idx = BITS(0, 3);
+                const u8 rdlo_idx = BITS(12, 15);
+                const u8 rdhi_idx = BITS(16, 19);
+                const bool do_swap = (BIT(5) == 1);
+
+                const u32 rdlo_val = state->Reg[rdlo_idx];
+                const u32 rdhi_val = state->Reg[rdhi_idx];
+                const u32 rn_val   = state->Reg[rn_idx];
+                u32 rm_val         = state->Reg[rm_idx];
+
+                if (do_swap)
+                    rm_val = (((rm_val & 0xFFFF) << 16) | (rm_val >> 16));
+
+                const s32 product1 = (s16)(rn_val & 0xFFFF) * (s16)(rm_val & 0xFFFF);
+                const s32 product2 = (s16)((rn_val >> 16) & 0xFFFF) * (s16)((rm_val >> 16) & 0xFFFF);
+                s64 result;
+
+                // SMLALD
+                if (BIT(6) == 0) {
+                    result = (product1 + product2) + (s64)(rdlo_val | ((s64)rdhi_val << 32));
+                }
+                // SMLSLD
+                else {
+                    result = (product1 - product2) + (s64)(rdlo_val | ((s64)rdhi_val << 32));
                 }
-                break;
-            }
-
-            Rm = ((state->Reg[BITS (0, 3)] >> ror) & 0xFF);
-            if (Rm & 0x80)
-                Rm |= 0xffffff00;
-
-            if (BITS (16, 19) == 0xf)
-                /* SXTB */
-                state->Reg[BITS (12, 15)] = Rm;
-            else
-                /* SXTAB */
-                state->Reg[BITS (12, 15)] += Rm;
-        }
-        return 1;
-
-        case 0x6b: {
-            ARMword Rm;
-            int ror = -1;
-
-            switch (BITS (4, 11)) {
-            case 0x07:
-                ror = 0;
-                break;
-            case 0x47:
-                ror = 8;
-                break;
-            case 0x87:
-                ror = 16;
-                break;
-            case 0xc7:
-                ror = 24;
-                break;
 
-            case 0xf3:
-                DEST = ((RHS & 0xFF) << 24) | ((RHS & 0xFF00)) << 8 | ((RHS & 0xFF0000) >> 8) | ((RHS & 0xFF000000) >> 24);
+                state->Reg[rdlo_idx] = (result & 0xFFFFFFFF);
+                state->Reg[rdhi_idx] = ((result >> 32) & 0xFFFFFFFF);
                 return 1;
-            case 0xfb:
-                DEST = ((RHS & 0xFF) << 8) | ((RHS & 0xFF00)) >> 8 | ((RHS & 0xFF0000) << 8) | ((RHS & 0xFF000000) >> 8);
-                return 1;
-            default:
-                break;
             }
+            break;
+        case 0x75: // SMMLA, SMMUL, and SMMLS
+            {
+                const u8 rm_idx = BITS(8, 11);
+                const u8 rn_idx = BITS(0, 3);
+                const u8 ra_idx = BITS(12, 15);
+                const u8 rd_idx = BITS(16, 19);
+                const bool do_round = (BIT(5) == 1);
 
-            if (ror == -1)
-                break;
+                const u32 rm_val = state->Reg[rm_idx];
+                const u32 rn_val = state->Reg[rn_idx];
 
-            Rm = ((state->Reg[BITS (0, 3)] >> ror) & 0xFFFF);
-            if (Rm & 0x8000)
-                Rm |= 0xffff0000;
+                // Assume SMMUL by default.
+                s64 result = (s64)(s32)rn_val * (s64)(s32)rm_val;
 
-            if (BITS (16, 19) == 0xf)
-                /* SXTH */
-                state->Reg[BITS (12, 15)] = Rm;
-            else
-                /* SXTAH */
-                state->Reg[BITS (12, 15)] = state->Reg[BITS (16, 19)] + Rm;
-        }
-        return 1;
+                if (ra_idx != 15) {
+                    const u32 ra_val = state->Reg[ra_idx];
 
-        case 0x6e: {
-            ARMword Rm;
-            int ror = -1;
+                    // SMMLA, otherwise SMMLS
+                    if (BIT(6) == 0)
+                        result += ((s64)ra_val << 32);
+                    else
+                        result = ((s64)ra_val << 32) - result;
+                }
 
-            switch (BITS (4, 11)) {
-            case 0x07:
-                ror = 0;
-                break;
-            case 0x47:
-                ror = 8;
-                break;
-            case 0x87:
-                ror = 16;
-                break;
-            case 0xc7:
-                ror = 24;
-                break;
+                if (do_round)
+                    result += 0x80000000;
 
-            case 0x01:
-            case 0xf3:
-                //ichfly
-                //USAT16
-                {
-                    u8 tar = BITS(12, 15);
-                    u8 src = BITS(0, 3);
-                    u8 val = BITS(16, 19);
-                    s16 a1 = (state->Reg[src]);
-                    s16 a2 = (state->Reg[src] >> 0x10);
-                    s16 max = 0xFFFF >> (16 - val);
-                    if (max < a1) a1 = max;
-                    if (max < a2) a2 = max;
-                    u32 temp2 = ((u32)(a2)) << 0x10;
-                    state->Reg[tar] = (a1 & 0xFFFF) | (temp2);
-                }
+                state->Reg[rd_idx] = ((result >> 32) & 0xFFFFFFFF);
                 return 1;
-            default:
-                break;
-            }
-
-            if (ror == -1) {
-                if (BITS (4, 6) == 0x7) {
-                    printf ("Unhandled v6 insn: usat\n");
-                    return 0;
-                }
-                break;
             }
+            break;
+        case 0x78:
+            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);
 
-            Rm = ((state->Reg[BITS (0, 3)] >> ror) & 0xFF);
-
-            if (BITS (16, 19) == 0xf)
-                /* UXTB */
-                state->Reg[BITS (12, 15)] = Rm;
-            else
-                /* UXTAB */
-                state->Reg[BITS (12, 15)] = state->Reg[BITS (16, 19)] + Rm;
-        }
-        return 1;
+                const u32 rm_val = state->Reg[rm_idx];
+                const u32 rn_val = state->Reg[rn_idx];
 
-        case 0x6f: {
-            ARMword Rm;
-            int ror = -1;
+                const u8 diff1 = ARMul_UnsignedAbsoluteDifference(rn_val & 0xFF, rm_val & 0xFF);
+                const u8 diff2 = ARMul_UnsignedAbsoluteDifference((rn_val >> 8) & 0xFF, (rm_val >> 8) & 0xFF);
+                const u8 diff3 = ARMul_UnsignedAbsoluteDifference((rn_val >> 16) & 0xFF, (rm_val >> 16) & 0xFF);
+                const u8 diff4 = ARMul_UnsignedAbsoluteDifference((rn_val >> 24) & 0xFF, (rm_val >> 24) & 0xFF);
 
-            switch (BITS (4, 11)) {
-            case 0x07:
-                ror = 0;
-                break;
-            case 0x47:
-                ror = 8;
-                break;
-            case 0x87:
-                ror = 16;
-                break;
-            case 0xc7:
-                ror = 24;
-                break;
+                u32 finalDif = (diff1 + diff2 + diff3 + diff4);
 
-            case 0xfb:
-                printf ("Unhandled v6 insn: revsh\n");
-                return 0;
-            default:
-                break;
-            }
+                // Op is USADA8 if true.
+                const u8 ra_idx = BITS(12, 15);
+                if (ra_idx != 15)
+                    finalDif += state->Reg[ra_idx];
 
-            if (ror == -1)
-                break;
-
-            Rm = ((state->Reg[BITS (0, 3)] >> ror) & 0xFFFF);
-
-            /* UXT */
-            /* state->Reg[BITS (12, 15)] = Rm; */
-            /* dyf add */
-            if (BITS (16, 19) == 0xf) {
-                state->Reg[BITS (12, 15)] = (Rm >> (8 * BITS(10, 11))) & 0x0000FFFF;
-            } else {
-                /* UXTAH */
-                /* state->Reg[BITS (12, 15)] = state->Reg [BITS (16, 19)] + Rm; */
-//            printf("rd is %x rn is %x rm is %x rotate is %x\n", state->Reg[BITS (12, 15)], state->Reg[BITS (16, 19)]
-//                   , Rm, BITS(10, 11));
-//            printf("icounter is %lld\n", state->NumInstrs);
-                state->Reg[BITS (12, 15)] = (state->Reg[BITS (16, 19)] >> (8 * (BITS(10, 11)))) + Rm;
-//        printf("rd is %x\n", state->Reg[BITS (12, 15)]);
-//        exit(-1);
+                state->Reg[rd_idx] = finalDif;
+                return 1;
             }
-        }
-        return 1;
-
-#if 0
+            break;
+        case 0x7a:
+            printf ("Unhandled v6 insn: usbfx\n");
+            break;
+        case 0x7c:
+            printf ("Unhandled v6 insn: bfc/bfi\n");
+            break;
         case 0x84:
             printf ("Unhandled v6 insn: srs\n");
             break;
-#endif
         default:
             break;
         }
         printf("Unhandled v6 insn: UNKNOWN: %08x %08X\n", instr, BITS(20, 27));
         return 0;
-    }
+    }
\ No newline at end of file
diff --git a/src/core/arm/interpreter/armsupp.cpp b/src/core/arm/interpreter/armsupp.cpp
index 30519f216..8b3661c8f 100644
--- a/src/core/arm/interpreter/armsupp.cpp
+++ b/src/core/arm/interpreter/armsupp.cpp
@@ -227,8 +227,9 @@ ARMul_CPSRAltered (ARMul_State * state)
     //state->Cpsr &= ~CBIT;
     ASSIGNV ((state->Cpsr & VBIT) != 0);
     //state->Cpsr &= ~VBIT;
-    ASSIGNS ((state->Cpsr & SBIT) != 0);
-    //state->Cpsr &= ~SBIT;
+    ASSIGNQ ((state->Cpsr & QBIT) != 0);
+    //state->Cpsr &= ~QBIT;
+    state->GEFlag = (state->Cpsr & 0x000F0000);
 #ifdef MODET
     ASSIGNT ((state->Cpsr & TBIT) != 0);
     //state->Cpsr &= ~TBIT;
@@ -391,6 +392,15 @@ ARMul_NthReg (ARMword instr, unsigned number)
     return (bit - 1);
 }
 
+/* Unsigned sum of absolute difference */
+u8 ARMul_UnsignedAbsoluteDifference(u8 left, u8 right)
+{
+        if (left > right)
+                return left - right;
+
+        return right - left;
+}
+
 /* Assigns the N and Z flags depending on the value of result.  */
 
 void
@@ -443,6 +453,14 @@ ARMul_AddOverflow (ARMul_State * state, ARMword a, ARMword b, ARMword result)
     ASSIGNV (AddOverflow (a, b, result));
 }
 
+/* Assigns the Q flag if the given result is considered an overflow from the addition of a and b  */
+void ARMul_AddOverflowQ(ARMul_State* state, ARMword a, ARMword b)
+{
+    u32 result = a + b;
+    if (((result ^ a) & (u32)0x80000000) && ((a ^ b) & (u32)0x80000000) == 0)
+        SETQ;
+}
+
 /* Assigns the C flag after an subtraction of a and b to give result.  */
 
 void
@@ -460,6 +478,107 @@ ARMul_SubOverflow (ARMul_State * state, ARMword a, ARMword b, ARMword result)
     ASSIGNV (SubOverflow (a, b, result));
 }
 
+/* 8-bit signed saturated addition */
+u8 ARMul_SignedSaturatedAdd8(u8 left, u8 right)
+{
+    u8 result = left + right;
+
+    if (((result ^ left) & 0x80) && ((left ^ right) & 0x80) == 0) {
+        if (left & 0x80)
+            result = 0x80;
+        else
+            result = 0x7F;
+    }
+
+    return result;
+}
+
+/* 8-bit signed saturated subtraction */
+u8 ARMul_SignedSaturatedSub8(u8 left, u8 right)
+{
+    u8 result = left - right;
+
+    if (((result ^ left) & 0x80) && ((left ^ right) & 0x80) != 0) {
+        if (left & 0x80)
+            result = 0x80;
+        else
+            result = 0x7F;
+    }
+
+    return result;
+}
+
+/* 16-bit signed saturated addition */
+u16 ARMul_SignedSaturatedAdd16(u16 left, u16 right)
+{
+    u16 result = left + right;
+
+    if (((result ^ left) & 0x8000) && ((left ^ right) & 0x8000) == 0) {
+        if (left & 0x8000)
+            result = 0x8000;
+        else
+            result = 0x7FFF;
+    }
+
+    return result;
+}
+
+/* 16-bit signed saturated subtraction */
+u16 ARMul_SignedSaturatedSub16(u16 left, u16 right)
+{
+    u16 result = left - right;
+
+    if (((result ^ left) & 0x8000) && ((left ^ right) & 0x8000) != 0) {
+        if (left & 0x8000)
+            result = 0x8000;
+        else
+            result = 0x7FFF;
+    }
+
+    return result;
+}
+
+/* 8-bit unsigned saturated addition */
+u8 ARMul_UnsignedSaturatedAdd8(u8 left, u8 right)
+{
+    u8 result = left + right;
+
+    if (result < left)
+        result = 0xFF;
+
+    return result;
+}
+
+/* 16-bit unsigned saturated addition */
+u16 ARMul_UnsignedSaturatedAdd16(u16 left, u16 right)
+{
+    u16 result = left + right;
+
+    if (result < left)
+        result = 0xFFFF;
+
+    return result;
+}
+
+/* 8-bit unsigned saturated subtraction */
+u8 ARMul_UnsignedSaturatedSub8(u8 left, u8 right)
+{
+    if (left <= right)
+        return 0;
+
+    return left - right;
+}
+
+/* 16-bit unsigned saturated subtraction */
+u16 ARMul_UnsignedSaturatedSub16(u16 left, u16 right)
+{
+    if (left <= right)
+        return 0;
+
+    return left - right;
+}
+
+
 /* This function does the work of generating the addresses used in an
    LDC instruction.  The code here is always post-indexed, it's up to the
    caller to get the input address correct and to handle base register
diff --git a/src/core/arm/skyeye_common/armdefs.h b/src/core/arm/skyeye_common/armdefs.h
index 8343aaa01..0f2bcbdb1 100644
--- a/src/core/arm/skyeye_common/armdefs.h
+++ b/src/core/arm/skyeye_common/armdefs.h
@@ -18,38 +18,26 @@
 #ifndef _ARMDEFS_H_
 #define _ARMDEFS_H_
 
-#include <stdio.h>
-#include <stdlib.h>
-#include <errno.h>
-
-#include "common/platform.h"
-
-//teawater add for arm2x86 2005.02.14-------------------------------------------
-// koodailar remove it for mingw 2005.12.18----------------
-//anthonylee modify it for portable 2007.01.30
-//#include "portable/mman.h"
+#include <cerrno>
+#include <csignal>
+#include <cstdio>
+#include <cstdlib>
+#include <cstring>
+#include <fcntl.h>
+#include <sys/stat.h>
+#include <sys/types.h>
 
 #include "arm_regformat.h"
+#include "common/common_types.h"
 #include "common/platform.h"
+#include "core/arm/skyeye_common/armmmu.h"
 #include "core/arm/skyeye_common/skyeye_defs.h"
 
-//AJ2D--------------------------------------------------------------------------
-
-//teawater add for arm2x86 2005.07.03-------------------------------------------
-
-#include <sys/types.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
 #if EMU_PLATFORM == PLATFORM_LINUX
+#include <sys/time.h>
 #include <unistd.h>
 #endif
-#include <errno.h>
-#include <sys/stat.h>
-#include <fcntl.h>
 
-//#include <memory_space.h>
-//AJ2D--------------------------------------------------------------------------
 #if 0
 #if 0
 #define DIFF_STATE 1
@@ -70,25 +58,8 @@
 #define LOWHIGH 1
 #define HIGHLOW 2
 
-//teawater add DBCT_TEST_SPEED 2005.10.04---------------------------------------
-#include <signal.h>
-
-#include "common/platform.h"
-
-#if EMU_PLATFORM == PLATFORM_LINUX
-#include <sys/time.h>
-#endif
-
 //#define DBCT_TEST_SPEED
 #define DBCT_TEST_SPEED_SEC    10
-//AJ2D--------------------------------------------------------------------------
-
-//teawater add compile switch for DBCT GDB RSP function 2005.10.21--------------
-//#define DBCT_GDBRSP
-//AJ2D--------------------------------------------------------------------------
-
-//#include <skyeye_defs.h>
-//#include <skyeye_types.h>
 
 #define ARM_BYTE_TYPE         0
 #define ARM_HALFWORD_TYPE     1
@@ -103,71 +74,34 @@
 typedef char *VoidStar;
 #endif
 
-typedef unsigned long long ARMdword;    /* must be 64 bits wide */
-typedef unsigned int ARMword;    /* must be 32 bits wide */
-typedef unsigned char ARMbyte;    /* must be 8 bits wide */
-typedef unsigned short ARMhword;    /* must be 16 bits wide */
+typedef u64 ARMdword;  // must be 64 bits wide
+typedef u32 ARMword;   // must be 32 bits wide
+typedef u16 ARMhword;  // must be 16 bits wide
+typedef u8 ARMbyte;    // must be 8 bits wide
 typedef struct ARMul_State ARMul_State;
 typedef struct ARMul_io ARMul_io;
 typedef struct ARMul_Energy ARMul_Energy;
 
-//teawater add for arm2x86 2005.06.24-------------------------------------------
-#include <stdint.h>
-//AJ2D--------------------------------------------------------------------------
-/*
-//chy 2005-05-11
-#ifndef __CYGWIN__
-//teawater add for arm2x86 2005.02.14-------------------------------------------
-typedef unsigned char           uint8_t;
-typedef unsigned short          uint16_t;
-typedef unsigned int            u32;
-#if defined (__x86_64__)
-typedef unsigned long           uint64_t;
-#else
-typedef unsigned long long      uint64_t;
-#endif
-////AJ2D--------------------------------------------------------------------------
-#endif
-*/
 
-#include "core/arm/skyeye_common/armmmu.h"
-//#include "lcd/skyeye_lcd.h"
-
-
-//#include "skyeye.h"
-//#include "skyeye_device.h"
-//#include "net/skyeye_net.h"
-//#include "skyeye_config.h"
-
-
-typedef unsigned ARMul_CPInits (ARMul_State * state);
-typedef unsigned ARMul_CPExits (ARMul_State * state);
-typedef unsigned ARMul_LDCs (ARMul_State * state, unsigned type,
-                 ARMword instr, ARMword value);
-typedef unsigned ARMul_STCs (ARMul_State * state, unsigned type,
-                 ARMword instr, ARMword * value);
-typedef unsigned ARMul_MRCs (ARMul_State * state, unsigned type,
-                 ARMword instr, ARMword * value);
-typedef unsigned ARMul_MCRs (ARMul_State * state, unsigned type,
-                 ARMword instr, ARMword value);
-typedef unsigned ARMul_MRRCs (ARMul_State * state, unsigned type,
-                 ARMword instr, ARMword * value1, ARMword * value2);
-typedef unsigned ARMul_MCRRs (ARMul_State * state, unsigned type,
-                 ARMword instr, ARMword value1, ARMword value2);
-typedef unsigned ARMul_CDPs (ARMul_State * state, unsigned type,
-                 ARMword instr);
-typedef unsigned ARMul_CPReads (ARMul_State * state, unsigned reg,
-                ARMword * value);
-typedef unsigned ARMul_CPWrites (ARMul_State * state, unsigned reg,
-                 ARMword value);
+typedef unsigned ARMul_CPInits(ARMul_State* state);
+typedef unsigned ARMul_CPExits(ARMul_State* state);
+typedef unsigned ARMul_LDCs(ARMul_State* state, unsigned type, ARMword instr, ARMword value);
+typedef unsigned ARMul_STCs(ARMul_State* state, unsigned type, ARMword instr, ARMword* value);
+typedef unsigned ARMul_MRCs(ARMul_State* state, unsigned type, ARMword instr, ARMword* value);
+typedef unsigned ARMul_MCRs(ARMul_State* state, unsigned type, ARMword instr, ARMword value);
+typedef unsigned ARMul_MRRCs(ARMul_State* state, unsigned type, ARMword instr, ARMword* value1, ARMword* value2);
+typedef unsigned ARMul_MCRRs(ARMul_State* state, unsigned type, ARMword instr, ARMword value1, ARMword value2);
+typedef unsigned ARMul_CDPs(ARMul_State* state, unsigned type, ARMword instr);
+typedef unsigned ARMul_CPReads(ARMul_State* state, unsigned reg, ARMword* value);
+typedef unsigned ARMul_CPWrites(ARMul_State* state, unsigned reg, ARMword value);
 
 
 //added by ksh,2004-3-5
 struct ARMul_io
 {
-    ARMword *instr;        //to display the current interrupt state
-    ARMword *net_flag;    //to judge if network is enabled
-    ARMword *net_int;    //netcard interrupt
+    ARMword *instr;      // to display the current interrupt state
+    ARMword *net_flag;   // to judge if network is enabled
+    ARMword *net_int;    // netcard interrupt
 
     //ywc,2004-04-01
     ARMword *ts_int;
@@ -180,17 +114,17 @@ struct ARMul_io
 /* added by ksh,2004-11-26,some energy profiling */
 struct ARMul_Energy
 {
-    int energy_prof;    /* <tktan>  BUG200103282109 : for energy profiling */
-    int enable_func_energy;    /* <tktan> BUG200105181702 */
+    int energy_prof;        /* <tktan>  BUG200103282109 : for energy profiling */
+    int enable_func_energy; /* <tktan> BUG200105181702 */
     char *func_energy;
-    int func_display;    /* <tktan> BUG200103311509 : for function call display */
+    int func_display;       /* <tktan> BUG200103311509 : for function call display */
     int func_disp_start;    /* <tktan> BUG200104191428 : to start func profiling */
-    char *start_func;    /* <tktan> BUG200104191428 */
+    char *start_func;       /* <tktan> BUG200104191428 */
 
-    FILE *outfile;        /* <tktan> BUG200105201531 : direct console to file */
+    FILE *outfile;          /* <tktan> BUG200105201531 : direct console to file */
     long long tcycle, pcycle;
     float t_energy;
-    void *cur_task;        /* <tktan> BUG200103291737 */
+    void *cur_task;         /* <tktan> BUG200103291737 */
     long long t_mem_cycle, t_idle_cycle, t_uart_cycle;
     long long p_mem_cycle, p_idle_cycle, p_uart_cycle;
     long long p_io_update_tcycle;
@@ -203,13 +137,12 @@ struct ARMul_Energy
 
 typedef struct mem_bank
 {
-    ARMword (*read_byte) (ARMul_State * state, ARMword addr);
-    void (*write_byte) (ARMul_State * state, ARMword addr, ARMword data);
-      ARMword (*read_halfword) (ARMul_State * state, ARMword addr);
-    void (*write_halfword) (ARMul_State * state, ARMword addr,
-                ARMword data);
-      ARMword (*read_word) (ARMul_State * state, ARMword addr);
-    void (*write_word) (ARMul_State * state, ARMword addr, ARMword data);
+    ARMword (*read_byte) (ARMul_State* state, ARMword addr);
+    void (*write_byte) (ARMul_State* state, ARMword addr, ARMword data);
+    ARMword (*read_halfword) (ARMul_State* state, ARMword addr);
+    void (*write_halfword) (ARMul_State* state, ARMword addr, ARMword data);
+    ARMword (*read_word) (ARMul_State* state, ARMword addr);
+    void (*write_word) (ARMul_State* state, ARMword addr, ARMword data);
     unsigned int addr, len;
     char filename[MAX_STR];
     unsigned type;        //chy 2003-09-21: maybe io,ram,rom
@@ -224,24 +157,24 @@ typedef struct
 #define VFP_REG_NUM 64
 struct ARMul_State
 {
-    ARMword Emulate;    /* to start and stop emulation */
-    unsigned EndCondition;    /* reason for stopping */
+    ARMword Emulate;       /* to start and stop emulation */
+    unsigned EndCondition; /* reason for stopping */
     unsigned ErrorCode;    /* type of illegal instruction */
 
     /* Order of the following register should not be modified */
-    ARMword Reg[16];    /* the current register file */
-    ARMword Cpsr;        /* the current psr */
+    ARMword Reg[16];      /* the current register file */
+    ARMword Cpsr;         /* the current psr */
     ARMword Spsr_copy;
     ARMword phys_pc;
     ARMword Reg_usr[2];
-    ARMword Reg_svc[2]; /* R13_SVC R14_SVC */
+    ARMword Reg_svc[2];   /* R13_SVC R14_SVC */
     ARMword Reg_abort[2]; /* R13_ABORT R14_ABORT */
     ARMword Reg_undef[2]; /* R13 UNDEF R14 UNDEF */
     ARMword Reg_irq[2];   /* R13_IRQ R14_IRQ */
     ARMword Reg_firq[7];  /* R8---R14 FIRQ */
-    ARMword Spsr[7];    /* the exception psr's */
-    ARMword Mode;        /* the current mode */
-    ARMword Bank;        /* the current register bank */
+    ARMword Spsr[7];      /* the exception psr's */
+    ARMword Mode;         /* the current mode */
+    ARMword Bank;         /* the current register bank */
     ARMword exclusive_tag;
     ARMword exclusive_state;
     ARMword exclusive_result;
@@ -265,7 +198,7 @@ struct ARMul_State
         //ARMword translate_pc;
 
     /* add armv6 flags dyf:2010-08-09 */
-    ARMword GEFlag, EFlag, AFlag, QFlags;
+    ARMword GEFlag, EFlag, AFlag, QFlag;
     //chy:2003-08-19, used in arm v5e|xscale
     ARMword SFlag;
 #ifdef MODET
@@ -281,38 +214,39 @@ struct ARMul_State
 
     ARMword currentexaddr;
     ARMword currentexval;
+    ARMword currentexvald;
     ARMword servaddr;
 
     unsigned NextInstr;
-    unsigned VectorCatch;    /* caught exception mask */
-    unsigned CallDebug;    /* set to call the debugger */
-    unsigned CanWatch;    /* set by memory interface if its willing to suffer the
-                   overhead of checking for watchpoints on each memory
-                   access */
+    unsigned VectorCatch;                   /* caught exception mask */
+    unsigned CallDebug;                     /* set to call the debugger */
+    unsigned CanWatch;                      /* set by memory interface if its willing to suffer the
+                                               overhead of checking for watchpoints on each memory
+                                               access */
     unsigned int StopHandle;
 
-    char *CommandLine;    /* Command Line from ARMsd */
-
-    ARMul_CPInits *CPInit[16];    /* coprocessor initialisers */
-    ARMul_CPExits *CPExit[16];    /* coprocessor finalisers */
-    ARMul_LDCs *LDC[16];    /* LDC instruction */
-    ARMul_STCs *STC[16];    /* STC instruction */
-    ARMul_MRCs *MRC[16];    /* MRC instruction */
-    ARMul_MCRs *MCR[16];    /* MCR instruction */
-    ARMul_MRRCs *MRRC[16];    /* MRRC instruction */
-    ARMul_MCRRs *MCRR[16];    /* MCRR instruction */
-    ARMul_CDPs *CDP[16];    /* CDP instruction */
-    ARMul_CPReads *CPRead[16];    /* Read CP register */
-    ARMul_CPWrites *CPWrite[16];    /* Write CP register */
-    unsigned char *CPData[16];    /* Coprocessor data */
+    char *CommandLine;                      /* Command Line from ARMsd */
+
+    ARMul_CPInits *CPInit[16];              /* coprocessor initialisers */
+    ARMul_CPExits *CPExit[16];              /* coprocessor finalisers */
+    ARMul_LDCs *LDC[16];                    /* LDC instruction */
+    ARMul_STCs *STC[16];                    /* STC instruction */
+    ARMul_MRCs *MRC[16];                    /* MRC instruction */
+    ARMul_MCRs *MCR[16];                    /* MCR instruction */
+    ARMul_MRRCs *MRRC[16];                  /* MRRC instruction */
+    ARMul_MCRRs *MCRR[16];                  /* MCRR instruction */
+    ARMul_CDPs *CDP[16];                    /* CDP instruction */
+    ARMul_CPReads *CPRead[16];              /* Read CP register */
+    ARMul_CPWrites *CPWrite[16];            /* Write CP register */
+    unsigned char *CPData[16];              /* Coprocessor data */
     unsigned char const *CPRegWords[16];    /* map of coprocessor register sizes */
 
-    unsigned EventSet;    /* the number of events in the queue */
-    unsigned int Now;    /* time to the nearest cycle */
-    struct EventNode **EventPtr;    /* the event list */
+    unsigned EventSet;                      /* the number of events in the queue */
+    unsigned int Now;                       /* time to the nearest cycle */
+    struct EventNode **EventPtr;            /* the event list */
 
-    unsigned Debug;        /* show instructions as they are executed */
-    unsigned NresetSig;    /* reset the processor */
+    unsigned Debug;                         /* show instructions as they are executed */
+    unsigned NresetSig;                     /* reset the processor */
     unsigned NfiqSig;
     unsigned NirqSig;
 
@@ -356,12 +290,12 @@ So, if lateabtSig=1, then it means Late Abort Model(Base Updated Abort Model)
 */
     unsigned lateabtSig;
 
-    ARMword Vector;        /* synthesize aborts in cycle modes */
-    ARMword Aborted;    /* sticky flag for aborts */
-    ARMword Reseted;    /* sticky flag for Reset */
+    ARMword Vector;              /* synthesize aborts in cycle modes */
+    ARMword Aborted;             /* sticky flag for aborts */
+    ARMword Reseted;             /* sticky flag for Reset */
     ARMword Inted, LastInted;    /* sticky flags for interrupts */
-    ARMword Base;        /* extra hand for base writeback */
-    ARMword AbortAddr;    /* to keep track of Prefetch aborts */
+    ARMword Base;                /* extra hand for base writeback */
+    ARMword AbortAddr;           /* to keep track of Prefetch aborts */
 
     const struct Dbg_HostosInterface *hostif;
 
@@ -378,7 +312,7 @@ So, if lateabtSig=1, then it means Late Abort Model(Base Updated Abort Model)
     //chy: 2003-08-11, for different arm core type
     unsigned is_v4;        /* Are we emulating a v4 architecture (or higher) ?  */
     unsigned is_v5;        /* Are we emulating a v5 architecture ?  */
-    unsigned is_v5e;    /* Are we emulating a v5e architecture ?  */
+    unsigned is_v5e;       /* Are we emulating a v5e architecture ?  */
     unsigned is_v6;        /* Are we emulating a v6 architecture ?  */
     unsigned is_v7;        /* Are we emulating a v7 architecture ?  */
     unsigned is_XScale;    /* Are we emulating an XScale architecture ?  */
@@ -387,51 +321,43 @@ So, if lateabtSig=1, then it means Late Abort Model(Base Updated Abort Model)
     //chy 2005-09-19
     unsigned is_pxa27x;    /* Are we emulating a Intel PXA27x co-processor ?  */
     //chy: seems only used in xscale's CP14
-    unsigned int LastTime;    /* Value of last call to ARMul_Time() */
+    unsigned int LastTime; /* Value of last call to ARMul_Time() */
     ARMword CP14R0_CCD;    /* used to count 64 clock cycles with CP14 R0 bit 3 set */
 
 
-//added by ksh:for handle different machs io 2004-3-5
+    //added by ksh:for handle different machs io 2004-3-5
     ARMul_io mach_io;
 
-/*added by ksh,2004-11-26,some energy profiling*/
+    /*added by ksh,2004-11-26,some energy profiling*/
     ARMul_Energy energy;
 
-//teawater add for next_dis 2004.10.27-----------------------
+    //teawater add for next_dis 2004.10.27-----------------------
     int disassemble;
-//AJ2D------------------------------------------
 
-//teawater add for arm2x86 2005.02.15-------------------------------------------
+
+    //teawater add for arm2x86 2005.02.15-------------------------------------------
     u32 trap;
     u32 tea_break_addr;
     u32 tea_break_ok;
     int tea_pc;
-//AJ2D--------------------------------------------------------------------------
-//teawater add for arm2x86 2005.07.03-------------------------------------------
-
-    /*
-     * 2007-01-24 removed the term-io functions by Anthony Lee,
-     * moved to "device/uart/skyeye_uart_stdio.c".
-     */
 
-//AJ2D--------------------------------------------------------------------------
-//teawater add for arm2x86 2005.07.05-------------------------------------------
+    //teawater add for arm2x86 2005.07.05-------------------------------------------
     //arm_arm A2-18
     int abort_model;    //0 Base Restored Abort Model, 1 the Early Abort Model, 2 Base Updated Abort Model 
-//AJ2D--------------------------------------------------------------------------
-//teawater change for return if running tb dirty 2005.07.09---------------------
+
+    //teawater change for return if running tb dirty 2005.07.09---------------------
     void *tb_now;
-//AJ2D--------------------------------------------------------------------------
 
-//teawater add for record reg value to ./reg.txt 2005.07.10---------------------
+
+    //teawater add for record reg value to ./reg.txt 2005.07.10---------------------
     FILE *tea_reg_fd;
-//AJ2D--------------------------------------------------------------------------
 
-/*added by ksh in 2005-10-1*/
+
+    /*added by ksh in 2005-10-1*/
     cpu_config_t *cpu;
     //mem_config_t *mem_bank;
 
-/* added LPC remap function */
+    /* added LPC remap function */
     int vector_remap_flag;
     u32 vector_remap_addr;
     u32 vector_remap_size;
@@ -486,17 +412,14 @@ typedef ARMul_State arm_core_t;
 #define ARM_Debug_Prop   0x10
 #define ARM_Isync_Prop   ARM_Debug_Prop
 #define ARM_Lock_Prop    0x20
-//chy 2003-08-11 
 #define ARM_v4_Prop      0x40
 #define ARM_v5_Prop      0x80
-/*jeff.du 2010-08-05 */
 #define ARM_v6_Prop      0xc0
 
 #define ARM_v5e_Prop     0x100
 #define ARM_XScale_Prop  0x200
 #define ARM_ep9312_Prop  0x400
 #define ARM_iWMMXt_Prop  0x800
-//chy 2005-09-19
 #define ARM_PXA27X_Prop  0x1000
 #define ARM_v7_Prop      0x2000
 
@@ -591,47 +514,44 @@ typedef ARMul_State arm_core_t;
 #ifdef __cplusplus
 extern "C" {
 #endif
-extern void ARMul_EmulateInit (void);
-extern void ARMul_Reset (ARMul_State * state);
+extern void ARMul_EmulateInit();
+extern void ARMul_Reset(ARMul_State* state);
 #ifdef __cplusplus
     }
 #endif
-extern ARMul_State *ARMul_NewState (ARMul_State * state);
-extern ARMword ARMul_DoProg (ARMul_State * state);
-extern ARMword ARMul_DoInstr (ARMul_State * state);
+extern ARMul_State *ARMul_NewState(ARMul_State* state);
+extern ARMword ARMul_DoProg(ARMul_State* state);
+extern ARMword ARMul_DoInstr(ARMul_State* state);
 /***************************************************************************\
 *                Definitons of things for event handling                    *
 \***************************************************************************/
 
-extern void ARMul_ScheduleEvent (ARMul_State * state, unsigned int delay,
-                 unsigned (*func) ());
-extern void ARMul_EnvokeEvent (ARMul_State * state);
-extern unsigned int ARMul_Time (ARMul_State * state);
+extern void ARMul_ScheduleEvent(ARMul_State* state, unsigned int delay, unsigned(*func) ());
+extern void ARMul_EnvokeEvent(ARMul_State* state);
+extern unsigned int ARMul_Time(ARMul_State* state);
 
 /***************************************************************************\
 *                          Useful support routines                          *
 \***************************************************************************/
 
-extern ARMword ARMul_GetReg (ARMul_State * state, unsigned mode,
-                 unsigned reg);
-extern void ARMul_SetReg (ARMul_State * state, unsigned mode, unsigned reg,
-              ARMword value);
-extern ARMword ARMul_GetPC (ARMul_State * state);
-extern ARMword ARMul_GetNextPC (ARMul_State * state);
-extern void ARMul_SetPC (ARMul_State * state, ARMword value);
-extern ARMword ARMul_GetR15 (ARMul_State * state);
-extern void ARMul_SetR15 (ARMul_State * state, ARMword value);
-
-extern ARMword ARMul_GetCPSR (ARMul_State * state);
-extern void ARMul_SetCPSR (ARMul_State * state, ARMword value);
-extern ARMword ARMul_GetSPSR (ARMul_State * state, ARMword mode);
-extern void ARMul_SetSPSR (ARMul_State * state, ARMword mode, ARMword value);
+extern ARMword ARMul_GetReg (ARMul_State* state, unsigned mode, unsigned reg);
+extern void ARMul_SetReg (ARMul_State* state, unsigned mode, unsigned reg, ARMword value);
+extern ARMword ARMul_GetPC(ARMul_State* state);
+extern ARMword ARMul_GetNextPC(ARMul_State* state);
+extern void ARMul_SetPC(ARMul_State* state, ARMword value);
+extern ARMword ARMul_GetR15(ARMul_State* state);
+extern void ARMul_SetR15(ARMul_State* state, ARMword value);
+
+extern ARMword ARMul_GetCPSR(ARMul_State* state);
+extern void ARMul_SetCPSR(ARMul_State* state, ARMword value);
+extern ARMword ARMul_GetSPSR(ARMul_State* state, ARMword mode);
+extern void ARMul_SetSPSR(ARMul_State* state, ARMword mode, ARMword value);
 
 /***************************************************************************\
 *                  Definitons of things to handle aborts                    *
 \***************************************************************************/
 
-extern void ARMul_Abort (ARMul_State * state, ARMword address);
+extern void ARMul_Abort(ARMul_State* state, ARMword address);
 #ifdef MODET
 #define ARMul_ABORTWORD (state->TFlag ? 0xefffdfff : 0xefffffff)    /* SWI -1 */
 #define ARMul_PREFETCHABORT(address) if (state->AbortAddr == 1) \
@@ -649,54 +569,40 @@ extern void ARMul_Abort (ARMul_State * state, ARMword address);
 *              Definitons of things in the memory interface                 *
 \***************************************************************************/
 
-extern unsigned ARMul_MemoryInit (ARMul_State * state,
-                  unsigned int initmemsize);
-extern void ARMul_MemoryExit (ARMul_State * state);
+extern unsigned ARMul_MemoryInit(ARMul_State* state, unsigned int initmemsize);
+extern void ARMul_MemoryExit(ARMul_State* state);
 
-extern ARMword ARMul_LoadInstrS (ARMul_State * state, ARMword address,
-                 ARMword isize);
-extern ARMword ARMul_LoadInstrN (ARMul_State * state, ARMword address,
-                 ARMword isize);
+extern ARMword ARMul_LoadInstrS(ARMul_State* state, ARMword address, ARMword isize);
+extern ARMword ARMul_LoadInstrN(ARMul_State* state, ARMword address, ARMword isize);
 #ifdef __cplusplus
 extern "C" {
 #endif
-extern ARMword ARMul_ReLoadInstr (ARMul_State * state, ARMword address,
-                  ARMword isize);
+extern ARMword ARMul_ReLoadInstr(ARMul_State* state, ARMword address, ARMword isize);
 #ifdef __cplusplus
     }
 #endif
-extern ARMword ARMul_LoadWordS (ARMul_State * state, ARMword address);
-extern ARMword ARMul_LoadWordN (ARMul_State * state, ARMword address);
-extern ARMword ARMul_LoadHalfWord (ARMul_State * state, ARMword address);
-extern ARMword ARMul_LoadByte (ARMul_State * state, ARMword address);
-
-extern void ARMul_StoreWordS (ARMul_State * state, ARMword address,
-                  ARMword data);
-extern void ARMul_StoreWordN (ARMul_State * state, ARMword address,
-                  ARMword data);
-extern void ARMul_StoreHalfWord (ARMul_State * state, ARMword address,
-                 ARMword data);
-extern void ARMul_StoreByte (ARMul_State * state, ARMword address,
-                 ARMword data);
-
-extern ARMword ARMul_SwapWord (ARMul_State * state, ARMword address,
-                   ARMword data);
-extern ARMword ARMul_SwapByte (ARMul_State * state, ARMword address,
-                   ARMword data);
-
-extern void ARMul_Icycles (ARMul_State * state, unsigned number,
-               ARMword address);
-extern void ARMul_Ccycles (ARMul_State * state, unsigned number,
-               ARMword address);
-
-extern ARMword ARMul_ReadWord (ARMul_State * state, ARMword address);
-extern ARMword ARMul_ReadByte (ARMul_State * state, ARMword address);
-extern void ARMul_WriteWord (ARMul_State * state, ARMword address,
-                 ARMword data);
-extern void ARMul_WriteByte (ARMul_State * state, ARMword address,
-                 ARMword data);
-
-extern ARMword ARMul_MemAccess (ARMul_State * state, ARMword, ARMword,
+extern ARMword ARMul_LoadWordS(ARMul_State* state, ARMword address);
+extern ARMword ARMul_LoadWordN(ARMul_State* state, ARMword address);
+extern ARMword ARMul_LoadHalfWord(ARMul_State* state, ARMword address);
+extern ARMword ARMul_LoadByte(ARMul_State* state, ARMword address);
+
+extern void ARMul_StoreWordS(ARMul_State* state, ARMword address, ARMword data);
+extern void ARMul_StoreWordN(ARMul_State* state, ARMword address, ARMword data);
+extern void ARMul_StoreHalfWord(ARMul_State* state, ARMword address, ARMword data);
+extern void ARMul_StoreByte(ARMul_State* state, ARMword address, ARMword data);
+
+extern ARMword ARMul_SwapWord(ARMul_State* state, ARMword address, ARMword data);
+extern ARMword ARMul_SwapByte(ARMul_State* state, ARMword address, ARMword data);
+
+extern void ARMul_Icycles(ARMul_State* state, unsigned number, ARMword address);
+extern void ARMul_Ccycles(ARMul_State* state, unsigned number, ARMword address);
+
+extern ARMword ARMul_ReadWord(ARMul_State* state, ARMword address);
+extern ARMword ARMul_ReadByte(ARMul_State* state, ARMword address);
+extern void ARMul_WriteWord(ARMul_State* state, ARMword address, ARMword data);
+extern void ARMul_WriteByte(ARMul_State* state, ARMword address, ARMword data);
+
+extern ARMword ARMul_MemAccess(ARMul_State* state, ARMword, ARMword,
                 ARMword, ARMword, ARMword, ARMword, ARMword,
                 ARMword, ARMword, ARMword);
 
@@ -739,66 +645,40 @@ extern ARMword ARMul_MemAccess (ARMul_State * state, ARMword, ARMword,
 #define ARMul_CP15_DBCON_E1     0x000c
 #define ARMul_CP15_DBCON_E0     0x0003
 
-extern unsigned ARMul_CoProInit (ARMul_State * state);
-extern void ARMul_CoProExit (ARMul_State * state);
-extern void ARMul_CoProAttach (ARMul_State * state, unsigned number,
-                   ARMul_CPInits * init, ARMul_CPExits * exit,
-                   ARMul_LDCs * ldc, ARMul_STCs * stc,
-                   ARMul_MRCs * mrc, ARMul_MCRs * mcr,
-                   ARMul_MRRCs * mrrc, ARMul_MCRRs * mcrr,
-                   ARMul_CDPs * cdp,
-                   ARMul_CPReads * read, ARMul_CPWrites * write);
-extern void ARMul_CoProDetach (ARMul_State * state, unsigned number);
+extern unsigned ARMul_CoProInit(ARMul_State* state);
+extern void ARMul_CoProExit(ARMul_State* state);
+extern void ARMul_CoProAttach (ARMul_State* state, unsigned number,
+                   ARMul_CPInits* init, ARMul_CPExits* exit,
+                   ARMul_LDCs* ldc, ARMul_STCs* stc,
+                   ARMul_MRCs* mrc, ARMul_MCRs* mcr,
+                   ARMul_MRRCs* mrrc, ARMul_MCRRs* mcrr,
+                   ARMul_CDPs* cdp,
+                   ARMul_CPReads* read, ARMul_CPWrites* write);
+extern void ARMul_CoProDetach(ARMul_State* state, unsigned number);
 
 /***************************************************************************\
 *               Definitons of things in the host environment                *
 \***************************************************************************/
 
-extern unsigned ARMul_OSInit (ARMul_State * state);
-extern void ARMul_OSExit (ARMul_State * state);
+extern unsigned ARMul_OSInit(ARMul_State* state);
+extern void ARMul_OSExit(ARMul_State* state);
 
 #ifdef __cplusplus
  extern "C" {
 #endif
 
-extern unsigned ARMul_OSHandleSWI (ARMul_State * state, ARMword number);
+extern unsigned ARMul_OSHandleSWI(ARMul_State* state, ARMword number);
 #ifdef __cplusplus
 }
 #endif
 
 
-extern ARMword ARMul_OSLastErrorP (ARMul_State * state);
+extern ARMword ARMul_OSLastErrorP(ARMul_State* state);
 
-extern ARMword ARMul_Debug (ARMul_State * state, ARMword pc, ARMword instr);
-extern unsigned ARMul_OSException (ARMul_State * state, ARMword vector,
-                   ARMword pc);
+extern ARMword ARMul_Debug(ARMul_State* state, ARMword pc, ARMword instr);
+extern unsigned ARMul_OSException(ARMul_State* state, ARMword vector, ARMword pc);
 extern int rdi_log;
 
-/***************************************************************************\
-*                            Host-dependent stuff                           *
-\***************************************************************************/
-
-#ifdef macintosh
-pascal void SpinCursor (short increment);    /* copied from CursorCtl.h */
-# define HOURGLASS           SpinCursor( 1 )
-# define HOURGLASS_RATE      1023    /* 2^n - 1 */
-#endif
-
-//teawater add for arm2x86 2005.02.14-------------------------------------------
-/*ywc 2005-03-31*/
-/*
-#include "arm2x86.h"
-#include "arm2x86_dp.h"
-#include "arm2x86_movl.h"
-#include "arm2x86_psr.h"
-#include "arm2x86_shift.h"
-#include "arm2x86_mem.h"
-#include "arm2x86_mul.h"
-#include "arm2x86_test.h"
-#include "arm2x86_other.h"
-#include "list.h"
-#include "tb.h"
-*/
 enum ConditionCode {
     EQ = 0,
     NE = 1,
@@ -851,32 +731,16 @@ enum ConditionCode {
 #define ZBIT_SHIFT    30
 #define CBIT_SHIFT    29
 #define VBIT_SHIFT    28
-#ifdef DBCT
-//teawater change for local tb branch directly jump 2005.10.18------------------
-#include "dbct/list.h"
-#include "dbct/arm2x86.h"
-#include "dbct/arm2x86_dp.h"
-#include "dbct/arm2x86_movl.h"
-#include "dbct/arm2x86_psr.h"
-#include "dbct/arm2x86_shift.h"
-#include "dbct/arm2x86_mem.h"
-#include "dbct/arm2x86_mul.h"
-#include "dbct/arm2x86_test.h"
-#include "dbct/arm2x86_other.h"
-#include "dbct/arm2x86_coproc.h"
-#include "dbct/tb.h"
-#endif
-//AJ2D--------------------------------------------------------------------------
-//AJ2D--------------------------------------------------------------------------
+
 #define SKYEYE_OUTREGS(fd) { fprintf ((fd), "R %x,%x,%x,%x,%x,%x,%x,%x,%x,%x,%x,%x,%x,%x,%x,%x,C %x,S %x,%x,%x,%x,%x,%x,%x,M %x,B %x,E %x,I %x,P %x,T %x,L %x,D %x,",\
                          state->Reg[0],state->Reg[1],state->Reg[2],state->Reg[3], \
                          state->Reg[4],state->Reg[5],state->Reg[6],state->Reg[7], \
                          state->Reg[8],state->Reg[9],state->Reg[10],state->Reg[11], \
                          state->Reg[12],state->Reg[13],state->Reg[14],state->Reg[15], \
-             state->Cpsr,   state->Spsr[0], state->Spsr[1], state->Spsr[2],\
+                         state->Cpsr,   state->Spsr[0], state->Spsr[1], state->Spsr[2],\
                          state->Spsr[3],state->Spsr[4], state->Spsr[5], state->Spsr[6],\
-             state->Mode,state->Bank,state->ErrorCode,state->instr,state->pc,\
-             state->temp,state->loaded,state->decoded);}
+                         state->Mode,state->Bank,state->ErrorCode,state->instr,state->pc,\
+                         state->temp,state->loaded,state->decoded);}
 
 #define SKYEYE_OUTMOREREGS(fd) { fprintf ((fd),"\
 RUs %x,%x,%x,%x,%x,%x,%x,%x,%x,%x,%x,%x,%x,%x,%x,%x,\
@@ -914,17 +778,28 @@ RUn %x,%x,%x,%x,%x,%x,%x,%x,%x,%x,%x,%x,%x,%x,%x,%x\n",\
 
 #define SA1110        0x6901b110
 #define SA1100        0x4401a100
-#define PXA250          0x69052100
-#define PXA270           0x69054110
-//#define PXA250              0x69052903
+#define PXA250        0x69052100
+#define PXA270        0x69054110
+//#define PXA250      0x69052903
 // 0x69052903;  //PXA250 B1 from intel 278522-001.pdf
 
 
-extern void ARMul_UndefInstr (ARMul_State *, ARMword);
-extern void ARMul_FixCPSR (ARMul_State *, ARMword, ARMword);
-extern void ARMul_FixSPSR (ARMul_State *, ARMword, ARMword);
-extern void ARMul_ConsolePrint (ARMul_State *, const char *, ...);
-extern void ARMul_SelectProcessor (ARMul_State *, unsigned);
+extern void ARMul_UndefInstr(ARMul_State*, ARMword);
+extern void ARMul_FixCPSR(ARMul_State*, ARMword, ARMword);
+extern void ARMul_FixSPSR(ARMul_State*, ARMword, ARMword);
+extern void ARMul_ConsolePrint(ARMul_State*, const char*, ...);
+extern void ARMul_SelectProcessor(ARMul_State*, unsigned);
+
+extern u8 ARMul_SignedSaturatedAdd8(u8, u8);
+extern u8 ARMul_SignedSaturatedSub8(u8, u8);
+extern u16 ARMul_SignedSaturatedAdd16(u16, u16);
+extern u16 ARMul_SignedSaturatedSub16(u16, u16);
+
+extern u8 ARMul_UnsignedSaturatedAdd8(u8, u8);
+extern u16 ARMul_UnsignedSaturatedAdd16(u16, u16);
+extern u8 ARMul_UnsignedSaturatedSub8(u8, u8);
+extern u16 ARMul_UnsignedSaturatedSub16(u16, u16);
+extern u8 ARMul_UnsignedAbsoluteDifference(u8, u8);
 
 #define DIFF_LOG 0
 #define SAVE_LOG 0
diff --git a/src/core/arm/skyeye_common/armemu.h b/src/core/arm/skyeye_common/armemu.h
index 7f7c0e682..3ea14b5a3 100644
--- a/src/core/arm/skyeye_common/armemu.h
+++ b/src/core/arm/skyeye_common/armemu.h
@@ -34,7 +34,7 @@
 #define ZBIT (1L << 30)
 #define CBIT (1L << 29)
 #define VBIT (1L << 28)
-#define SBIT (1L << 27)
+#define QBIT (1L << 27)
 #define IBIT (1L << 7)
 #define FBIT (1L << 6)
 #define IFBITS (3L << 6)
@@ -156,13 +156,14 @@
 #define R15PCMODE (state->Reg[15] & (R15PCBITS | R15MODEBITS))
 #define R15MODE (state->Reg[15] & R15MODEBITS)
 
-#define ECC ((NFLAG << 31) | (ZFLAG << 30) | (CFLAG << 29) | (VFLAG << 28) | (SFLAG << 27))
+#define ECC ((NFLAG << 31) | (ZFLAG << 30) | (CFLAG << 29) | (VFLAG << 28) | (QFLAG << 27))
 #define EINT (IFFLAGS << 6)
 #define ER15INT (IFFLAGS << 26)
 #define EMODE (state->Mode)
+#define EGEBITS (state->GEFlag & 0x000F0000)
 
 #ifdef MODET
-#define CPSR (ECC | EINT | EMODE | (TFLAG << 5))
+#define CPSR (ECC | EGEBITS | (EFLAG << 9) | (AFLAG << 8) | EINT | (TFLAG << 5) | EMODE)
 #else
 #define CPSR (ECC | EINT | EMODE)
 #endif
@@ -601,6 +602,7 @@ extern ARMword ARMul_SwitchMode (ARMul_State *, ARMword, ARMword);
 extern void ARMul_MSRCpsr (ARMul_State *, ARMword, ARMword);
 extern void ARMul_SubOverflow (ARMul_State *, ARMword, ARMword, ARMword);
 extern void ARMul_AddOverflow (ARMul_State *, ARMword, ARMword, ARMword);
+extern void ARMul_AddOverflowQ(ARMul_State*, ARMword, ARMword);
 extern void ARMul_SubCarry (ARMul_State *, ARMword, ARMword, ARMword);
 extern void ARMul_AddCarry (ARMul_State *, ARMword, ARMword, ARMword);
 extern tdstate ARMul_ThumbDecode (ARMul_State *, ARMword, ARMword, ARMword *);
diff --git a/src/core/arm/skyeye_common/vfp/vfp.cpp b/src/core/arm/skyeye_common/vfp/vfp.cpp
index 454f60099..5c036caeb 100644
--- a/src/core/arm/skyeye_common/vfp/vfp.cpp
+++ b/src/core/arm/skyeye_common/vfp/vfp.cpp
@@ -32,8 +32,7 @@
 
 //ARMul_State* persistent_state; /* function calls from SoftFloat lib don't have an access to ARMul_state. */
 
-unsigned
-VFPInit (ARMul_State *state)
+unsigned VFPInit(ARMul_State* state)
 {
     state->VFP[VFP_OFFSET(VFP_FPSID)] = VFP_FPSID_IMPLMEN<<24 | VFP_FPSID_SW<<23 | VFP_FPSID_SUBARCH<<16 |
                                         VFP_FPSID_PARTNUM<<8 | VFP_FPSID_VARIANT<<4 | VFP_FPSID_REVISION;
@@ -46,8 +45,7 @@ VFPInit (ARMul_State *state)
     return 0;
 }
 
-unsigned
-VFPMRC (ARMul_State * state, unsigned type, u32 instr, u32 * value)
+unsigned VFPMRC(ARMul_State* state, unsigned type, u32 instr, u32* value)
 {
     /* MRC<c> <coproc>,<opc1>,<Rt>,<CRn>,<CRm>{,<opc2>} */
     int CoProc = BITS (8, 11); /* 10 or 11 */
@@ -61,10 +59,21 @@ VFPMRC (ARMul_State * state, unsigned type, u32 instr, u32 * value)
 
     /* CRn/opc1 CRm/opc2 */
 
-    if (CoProc == 10 || CoProc == 11) {
-#define VFP_MRC_TRANS
-#include "core/arm/skyeye_common/vfp/vfpinstr.cpp"
-#undef VFP_MRC_TRANS
+    if (CoProc == 10 || CoProc == 11)
+    {
+        if (OPC_1 == 0x0 && CRm == 0 && (OPC_2 & 0x3) == 0)
+        {
+            /* VMOV r to s */
+            /* Transfering Rt is not mandatory, as the value of interest is pointed by value */
+            VMOVBRS(state, BIT(20), Rt, BIT(7)|CRn<<1, value);
+            return ARMul_DONE;
+        }
+
+        if (OPC_1 == 0x7 && CRm == 0 && OPC_2 == 0)
+        {
+            VMRS(state, CRn, Rt, value);
+            return ARMul_DONE;
+        }
     }
     DEBUG("Can't identify %x, CoProc %x, OPC_1 %x, Rt %x, CRn %x, CRm %x, OPC_2 %x\n",
           instr, CoProc, OPC_1, Rt, CRn, CRm, OPC_2);
@@ -72,8 +81,7 @@ VFPMRC (ARMul_State * state, unsigned type, u32 instr, u32 * value)
     return ARMul_CANT;
 }
 
-unsigned
-VFPMCR (ARMul_State * state, unsigned type, u32 instr, u32 value)
+unsigned VFPMCR(ARMul_State* state, unsigned type, u32 instr, u32 value)
 {
     /* MCR<c> <coproc>,<opc1>,<Rt>,<CRn>,<CRm>{,<opc2>} */
     int CoProc = BITS (8, 11); /* 10 or 11 */
@@ -86,10 +94,33 @@ VFPMCR (ARMul_State * state, unsigned type, u32 instr, u32 value)
     /* TODO check access permission */
 
     /* CRn/opc1 CRm/opc2 */
-    if (CoProc == 10 || CoProc == 11) {
-#define VFP_MCR_TRANS
-#include "core/arm/skyeye_common/vfp/vfpinstr.cpp"
-#undef VFP_MCR_TRANS
+    if (CoProc == 10 || CoProc == 11)
+    {
+        if (OPC_1 == 0x0 && CRm == 0 && (OPC_2 & 0x3) == 0)
+        {
+            /* VMOV s to r */
+            /* Transfering Rt is not mandatory, as the value of interest is pointed by value */
+            VMOVBRS(state, BIT(20), Rt, BIT(7)|CRn<<1, &value);
+            return ARMul_DONE;
+        }
+
+        if (OPC_1 == 0x7 && CRm == 0 && OPC_2 == 0)
+        {
+            VMSR(state, CRn, Rt);
+            return ARMul_DONE;
+        }
+
+        if ((OPC_1 & 0x4) == 0 && CoProc == 11 && CRm == 0)
+        {
+            VFP_DEBUG_UNIMPLEMENTED(VMOVBRC);
+            return ARMul_DONE;
+        }
+
+        if (CoProc == 11 && CRm == 0)
+        {
+            VFP_DEBUG_UNIMPLEMENTED(VMOVBCR);
+            return ARMul_DONE;
+        }
     }
     DEBUG("Can't identify %x, CoProc %x, OPC_1 %x, Rt %x, CRn %x, CRm %x, OPC_2 %x\n",
           instr, CoProc, OPC_1, Rt, CRn, CRm, OPC_2);
@@ -97,8 +128,7 @@ VFPMCR (ARMul_State * state, unsigned type, u32 instr, u32 value)
     return ARMul_CANT;
 }
 
-unsigned
-VFPMRRC (ARMul_State * state, unsigned type, u32 instr, u32 * value1, u32 * value2)
+unsigned VFPMRRC(ARMul_State* state, unsigned type, u32 instr, u32* value1, u32* value2)
 {
     /* MCRR<c> <coproc>,<opc1>,<Rt>,<Rt2>,<CRm> */
     int CoProc = BITS (8, 11); /* 10 or 11 */
@@ -107,10 +137,20 @@ VFPMRRC (ARMul_State * state, unsigned type, u32 instr, u32 * value1, u32 * valu
     int Rt2 = BITS (16, 19);
     int CRm = BITS (0, 3);
 
-    if (CoProc == 10 || CoProc == 11) {
-#define VFP_MRRC_TRANS
-#include "core/arm/skyeye_common/vfp/vfpinstr.cpp"
-#undef VFP_MRRC_TRANS
+    if (CoProc == 10 || CoProc == 11)
+    {
+        if (CoProc == 10 && (OPC_1 & 0xD) == 1)
+        {
+            VFP_DEBUG_UNIMPLEMENTED(VMOVBRRSS);
+            return ARMul_DONE;
+        }
+
+        if (CoProc == 11 && (OPC_1 & 0xD) == 1)
+        {
+            /* Transfering Rt and Rt2 is not mandatory, as the value of interest is pointed by value1 and value2 */
+            VMOVBRRD(state, BIT(20), Rt, Rt2, BIT(5)<<4|CRm, value1, value2);
+            return ARMul_DONE;
+        }
     }
     DEBUG("Can't identify %x, CoProc %x, OPC_1 %x, Rt %x, Rt2 %x, CRm %x\n",
           instr, CoProc, OPC_1, Rt, Rt2, CRm);
@@ -118,8 +158,7 @@ VFPMRRC (ARMul_State * state, unsigned type, u32 instr, u32 * value1, u32 * valu
     return ARMul_CANT;
 }
 
-unsigned
-VFPMCRR (ARMul_State * state, unsigned type, u32 instr, u32 value1, u32 value2)
+unsigned VFPMCRR(ARMul_State* state, unsigned type, u32 instr, u32 value1, u32 value2)
 {
     /* MCRR<c> <coproc>,<opc1>,<Rt>,<Rt2>,<CRm> */
     int CoProc = BITS (8, 11); /* 10 or 11 */
@@ -132,10 +171,20 @@ VFPMCRR (ARMul_State * state, unsigned type, u32 instr, u32 value1, u32 value2)
 
     /* CRn/opc1 CRm/opc2 */
 
-    if (CoProc == 11 || CoProc == 10) {
-#define VFP_MCRR_TRANS
-#include "core/arm/skyeye_common/vfp/vfpinstr.cpp"
-#undef VFP_MCRR_TRANS
+    if (CoProc == 11 || CoProc == 10)
+    {
+        if (CoProc == 10 && (OPC_1 & 0xD) == 1)
+        {
+            VFP_DEBUG_UNIMPLEMENTED(VMOVBRRSS);
+            return ARMul_DONE;
+        }
+
+        if (CoProc == 11 && (OPC_1 & 0xD) == 1)
+        {
+            /* Transfering Rt and Rt2 is not mandatory, as the value of interest is pointed by value1 and value2 */
+            VMOVBRRD(state, BIT(20), Rt, Rt2, BIT(5)<<4|CRm, &value1, &value2);
+            return ARMul_DONE;
+        }
     }
     DEBUG("Can't identify %x, CoProc %x, OPC_1 %x, Rt %x, Rt2 %x, CRm %x\n",
           instr, CoProc, OPC_1, Rt, Rt2, CRm);
@@ -143,8 +192,7 @@ VFPMCRR (ARMul_State * state, unsigned type, u32 instr, u32 value1, u32 value2)
     return ARMul_CANT;
 }
 
-unsigned
-VFPSTC (ARMul_State * state, unsigned type, u32 instr, u32 * value)
+unsigned VFPSTC(ARMul_State* state, unsigned type, u32 instr, u32 * value)
 {
     /* STC{L}<c> <coproc>,<CRd>,[<Rn>],<option> */
     int CoProc = BITS (8, 11); /* 10 or 11 */
@@ -175,9 +223,17 @@ VFPSTC (ARMul_State * state, unsigned type, u32 instr, u32 * value)
         }
 #endif
 
-#define VFP_STC_TRANS
-#include "core/arm/skyeye_common/vfp/vfpinstr.cpp"
-#undef VFP_STC_TRANS
+        if (P == 1 && W == 0)
+        {
+            return VSTR(state, type, instr, value);
+        }
+
+        if (P == 1 && U == 0 && W == 1 && Rn == 0xD)
+        {
+            return VPUSH(state, type, instr, value);
+        }
+
+        return VSTM(state, type, instr, value);
     }
     DEBUG("Can't identify %x, CoProc %x, CRd %x, Rn %x, imm8 %x, P %x, U %x, D %x, W %x\n",
           instr, CoProc, CRd, Rn, imm8, P, U, D, W);
@@ -185,8 +241,7 @@ VFPSTC (ARMul_State * state, unsigned type, u32 instr, u32 * value)
     return ARMul_CANT;
 }
 
-unsigned
-VFPLDC (ARMul_State * state, unsigned type, u32 instr, u32 value)
+unsigned VFPLDC(ARMul_State* state, unsigned type, u32 instr, u32 value)
 {
     /* LDC{L}<c> <coproc>,<CRd>,[<Rn>] */
     int CoProc = BITS (8, 11); /* 10 or 11 */
@@ -204,10 +259,19 @@ VFPLDC (ARMul_State * state, unsigned type, u32 instr, u32 value)
         DEBUG("In %s, UNDEFINED\n", __FUNCTION__);
         exit(-1);
     }
-    if (CoProc == 10 || CoProc == 11) {
-#define VFP_LDC_TRANS
-#include "core/arm/skyeye_common/vfp/vfpinstr.cpp"
-#undef VFP_LDC_TRANS
+    if (CoProc == 10 || CoProc == 11)
+    {
+        if (P == 1 && W == 0)
+        {
+            return VLDR(state, type, instr, value);
+        }
+
+        if (P == 0 && U == 1 && W == 1 && Rn == 0xD)
+        {
+            return VPOP(state, type, instr, value);
+        }
+
+        return VLDM(state, type, instr, value);
     }
     DEBUG("Can't identify %x, CoProc %x, CRd %x, Rn %x, imm8 %x, P %x, U %x, D %x, W %x\n",
           instr, CoProc, CRd, Rn, imm8, P, U, D, W);
@@ -215,8 +279,7 @@ VFPLDC (ARMul_State * state, unsigned type, u32 instr, u32 value)
     return ARMul_CANT;
 }
 
-unsigned
-VFPCDP (ARMul_State * state, unsigned type, u32 instr)
+unsigned VFPCDP(ARMul_State* state, unsigned type, u32 instr)
 {
     /* CDP<c> <coproc>,<opc1>,<CRd>,<CRn>,<CRm>,<opc2> */
     int CoProc = BITS (8, 11); /* 10 or 11 */
@@ -275,10 +338,83 @@ VFPCDP (ARMul_State * state, unsigned type, u32 instr)
 
     /* CRn/opc1 CRm/opc2 */
 
-    if (CoProc == 10 || CoProc == 11) {
-#define VFP_CDP_TRANS
-#include "core/arm/skyeye_common/vfp/vfpinstr.cpp"
-#undef VFP_CDP_TRANS
+    if (CoProc == 10 || CoProc == 11)
+    {
+        if ((OPC_1 & 0xB) == 0 && (OPC_2 & 0x2) == 0)
+            DBG("VMLA :\n");
+
+        if ((OPC_1 & 0xB) == 0 && (OPC_2 & 0x2) == 2)
+            DBG("VMLS :\n");
+
+        if ((OPC_1 & 0xB) == 1 && (OPC_2 & 0x2) == 2)
+            DBG("VNMLA :\n");
+
+        if ((OPC_1 & 0xB) == 1 && (OPC_2 & 0x2) == 0)
+            DBG("VNMLS :\n");
+
+        if ((OPC_1 & 0xB) == 2 && (OPC_2 & 0x2) == 2)
+            DBG("VNMUL :\n");
+
+        if ((OPC_1 & 0xB) == 2 && (OPC_2 & 0x2) == 0)
+            DBG("VMUL :\n");
+
+        if ((OPC_1 & 0xB) == 3 && (OPC_2 & 0x2) == 0)
+            DBG("VADD :\n");
+
+        if ((OPC_1 & 0xB) == 3 && (OPC_2 & 0x2) == 2)
+            DBG("VSUB :\n");
+
+        if ((OPC_1 & 0xB) == 0xA && (OPC_2 & 0x2) == 0)
+            DBG("VDIV :\n");
+
+        if ((OPC_1 & 0xB) == 0xB && BITS(4, 7) == 0)
+        {
+            unsigned int single   = BIT(8) == 0;
+            unsigned int d        = (single ? BITS(12,15)<<1 | BIT(22) : BITS(12,15) | BIT(22)<<4);
+            unsigned int imm;
+            instr = BITS(16, 19) << 4 | BITS(0, 3); /* FIXME dirty workaround to get a correct imm */
+
+            if (single)
+                imm = BIT(7)<<31 | (BIT(6)==0)<<30 | (BIT(6) ? 0x1f : 0)<<25 | BITS(0, 5)<<19;
+            else
+                imm = BIT(7)<<31 | (BIT(6)==0)<<30 | (BIT(6) ? 0xff : 0)<<22 | BITS(0, 5)<<16;
+
+            VMOVI(state, single, d, imm);
+            return ARMul_DONE;
+        }
+
+        if ((OPC_1 & 0xB) == 0xB && CRn == 0 && (OPC_2 & 0x6) == 0x2)
+        {
+            unsigned int single   = BIT(8) == 0;
+            unsigned int d        = (single ? BITS(12,15)<<1 | BIT(22) : BITS(12,15) | BIT(22)<<4);
+            unsigned int m        = (single ? BITS( 0, 3)<<1 | BIT( 5) : BITS( 0, 3) | BIT( 5)<<4);;
+            VMOVR(state, single, d, m);
+            return ARMul_DONE;
+        }
+
+        if ((OPC_1 & 0xB) == 0xB && CRn == 0 && (OPC_2 & 0x7) == 6)
+            DBG("VABS :\n");
+
+        if ((OPC_1 & 0xB) == 0xB && CRn == 1 && (OPC_2 & 0x7) == 2)
+            DBG("VNEG :\n");
+
+        if ((OPC_1 & 0xB) == 0xB && CRn == 1 && (OPC_2 & 0x7) == 6)
+            DBG("VSQRT :\n");
+
+        if ((OPC_1 & 0xB) == 0xB && CRn == 4 && (OPC_2 & 0x2) == 2)
+            DBG("VCMP(1) :\n");
+
+        if ((OPC_1 & 0xB) == 0xB && CRn == 5 && (OPC_2 & 0x2) == 2 && CRm == 0)
+            DBG("VCMP(2) :\n");
+
+        if ((OPC_1 & 0xB) == 0xB && CRn == 7 && (OPC_2 & 0x6) == 6)
+            DBG("VCVT(BDS) :\n");
+
+        if ((OPC_1 & 0xB) == 0xB && CRn >= 0xA && (OPC_2 & 0x2) == 2)
+            DBG("VCVT(BFF) :\n");
+
+        if ((OPC_1 & 0xB) == 0xB && CRn > 7 && (OPC_2 & 0x2) == 2)
+            DBG("VCVT(BFI) :\n");
 
         int exceptions = 0;
         if (CoProc == 10)
@@ -296,23 +432,93 @@ VFPCDP (ARMul_State * state, unsigned type, u32 instr)
 
 
 /* ----------- MRC ------------ */
-#define VFP_MRC_IMPL
-#include "core/arm/skyeye_common/vfp/vfpinstr.cpp"
-#undef VFP_MRC_IMPL
-
-#define VFP_MRRC_IMPL
-#include "core/arm/skyeye_common/vfp/vfpinstr.cpp"
-#undef VFP_MRRC_IMPL
-
+void VMOVBRS(ARMul_State* state, ARMword to_arm, ARMword t, ARMword n, ARMword* value)
+{
+    DBG("VMOV(BRS) :\n");
+    if (to_arm)
+    {
+        DBG("\tr%d <= s%d=[%x]\n", t, n, state->ExtReg[n]);
+        *value = state->ExtReg[n];
+    }
+    else
+    {
+        DBG("\ts%d <= r%d=[%x]\n", n, t, *value);
+        state->ExtReg[n] = *value;
+    }
+}
+void VMRS(ARMul_State* state, ARMword reg, ARMword Rt, ARMword* value)
+{
+    DBG("VMRS :");
+    if (reg == 1)
+    {
+        if (Rt != 15)
+        {
+            *value = state->VFP[VFP_OFFSET(VFP_FPSCR)];
+            DBG("\tr%d <= fpscr[%08x]\n", Rt, state->VFP[VFP_OFFSET(VFP_FPSCR)]);
+        }
+        else
+        {
+            *value = state->VFP[VFP_OFFSET(VFP_FPSCR)] ;
+            DBG("\tflags <= fpscr[%1xxxxxxxx]\n", state->VFP[VFP_OFFSET(VFP_FPSCR)]>>28);
+        }
+    }
+    else
+    {
+        switch (reg)
+        {
+            case 0:
+                *value = state->VFP[VFP_OFFSET(VFP_FPSID)];
+                DBG("\tr%d <= fpsid[%08x]\n", Rt, state->VFP[VFP_OFFSET(VFP_FPSID)]);
+                break;
+            case 6:
+                /* MVFR1, VFPv3 only ? */
+                DBG("\tr%d <= MVFR1 unimplemented\n", Rt);
+                break;
+            case 7:
+                /* MVFR0, VFPv3 only? */
+                DBG("\tr%d <= MVFR0 unimplemented\n", Rt);
+                break;
+            case 8:
+                *value = state->VFP[VFP_OFFSET(VFP_FPEXC)];
+                DBG("\tr%d <= fpexc[%08x]\n", Rt, state->VFP[VFP_OFFSET(VFP_FPEXC)]);
+                break;
+            default:
+                DBG("\tSUBARCHITECTURE DEFINED\n");
+                break;
+        }
+    }
+}
+void VMOVBRRD(ARMul_State* state, ARMword to_arm, ARMword t, ARMword t2, ARMword n, ARMword* value1, ARMword* value2)
+{
+    DBG("VMOV(BRRD) :\n");
+    if (to_arm)
+    {
+        DBG("\tr[%d-%d] <= s[%d-%d]=[%x-%x]\n", t2, t, n*2+1, n*2, state->ExtReg[n*2+1], state->ExtReg[n*2]);
+        *value2 = state->ExtReg[n*2+1];
+        *value1 = state->ExtReg[n*2];
+    }
+    else
+    {
+        DBG("\ts[%d-%d] <= r[%d-%d]=[%x-%x]\n", n*2+1, n*2, t2, t, *value2, *value1);
+        state->ExtReg[n*2+1] = *value2;
+        state->ExtReg[n*2] = *value1;
+    }
+}
 
 /* ----------- MCR ------------ */
-#define VFP_MCR_IMPL
-#include "core/arm/skyeye_common/vfp/vfpinstr.cpp"
-#undef VFP_MCR_IMPL
-
-#define VFP_MCRR_IMPL
-#include "core/arm/skyeye_common/vfp/vfpinstr.cpp"
-#undef VFP_MCRR_IMPL
+void VMSR(ARMul_State* state, ARMword reg, ARMword Rt)
+{
+    if (reg == 1)
+    {
+        DBG("VMSR :\tfpscr <= r%d=[%x]\n", Rt, state->Reg[Rt]);
+        state->VFP[VFP_OFFSET(VFP_FPSCR)] = state->Reg[Rt];
+    }
+    else if (reg == 8)
+    {
+        DBG("VMSR :\tfpexc <= r%d=[%x]\n", Rt, state->Reg[Rt]);
+        state->VFP[VFP_OFFSET(VFP_FPEXC)] = state->Reg[Rt];
+    }
+}
 
 /* Memory operation are not inlined, as old Interpreter and Fast interpreter
    don't have the same memory operation interface.
@@ -322,21 +528,342 @@ VFPCDP (ARMul_State * state, unsigned type, u32 instr)
    of vfp instructions in old interpreter and fast interpreter are separate. */
 
 /* ----------- STC ------------ */
-#define VFP_STC_IMPL
-#include "core/arm/skyeye_common/vfp/vfpinstr.cpp"
-#undef VFP_STC_IMPL
+int VSTR(ARMul_State* state, int type, ARMword instr, ARMword* value)
+{
+    static int i = 0;
+    static int single_reg, add, d, n, imm32, regs;
+    if (type == ARMul_FIRST)
+    {
+        single_reg = BIT(8) == 0;       /* Double precision */
+        add = BIT(23);          /* */
+        imm32 = BITS(0,7)<<2;   /* may not be used */
+        d = single_reg ? BITS(12, 15)<<1|BIT(22) : BIT(22)<<4|BITS(12, 15); /* Base register */
+        n = BITS(16, 19);       /* destination register */
 
+        DBG("VSTR :\n");
+
+        i = 0;
+        regs = 1;
+
+        return ARMul_DONE;
+    }
+    else if (type == ARMul_DATA)
+    {
+        if (single_reg)
+        {
+            *value = state->ExtReg[d+i];
+            DBG("\taddr[?] <= s%d=[%x]\n", d+i, state->ExtReg[d+i]);
+            i++;
+            if (i < regs)
+                return ARMul_INC;
+            else
+                return ARMul_DONE;
+        }
+        else
+        {
+            /* FIXME Careful of endianness, may need to rework this */
+            *value = state->ExtReg[d*2+i];
+            DBG("\taddr[?] <= s[%d]=[%x]\n", d*2+i, state->ExtReg[d*2+i]);
+            i++;
+            if (i < regs*2)
+                return ARMul_INC;
+            else
+                return ARMul_DONE;
+        }
+    }
+
+    return -1;
+}
+int VPUSH(ARMul_State* state, int type, ARMword instr, ARMword* value)
+{
+    static int i = 0;
+    static int single_regs, add, wback, d, n, imm32, regs;
+    if (type == ARMul_FIRST)
+    {
+        single_regs = BIT(8) == 0;      /* Single precision */
+        d = single_regs ? BITS(12, 15)<<1|BIT(22) : BIT(22)<<4|BITS(12, 15); /* Base register */
+        imm32 = BITS(0,7)<<2;   /* may not be used */
+        regs = single_regs ? BITS(0, 7) : BITS(1, 7); /* FSTMX if regs is odd */
+
+        DBG("VPUSH :\n");
+        DBG("\tsp[%x]", state->Reg[R13]);
+        state->Reg[R13] = state->Reg[R13] - imm32;
+        DBG("=>[%x]\n", state->Reg[R13]);
+
+        i = 0;
+
+        return ARMul_DONE;
+    }
+    else if (type == ARMul_DATA)
+    {
+        if (single_regs)
+        {
+            *value = state->ExtReg[d + i];
+            DBG("\taddr[?] <= s%d=[%x]\n", d+i, state->ExtReg[d + i]);
+            i++;
+            if (i < regs)
+                return ARMul_INC;
+            else
+                return ARMul_DONE;
+        }
+        else
+        {
+            /* FIXME Careful of endianness, may need to rework this */
+            *value = state->ExtReg[d*2 + i];
+            DBG("\taddr[?] <= s[%d]=[%x]\n", d*2 + i, state->ExtReg[d*2 + i]);
+            i++;
+            if (i < regs*2)
+                return ARMul_INC;
+            else
+                return ARMul_DONE;
+        }
+    }
+
+    return -1;
+}
+int VSTM(ARMul_State* state, int type, ARMword instr, ARMword* value)
+{
+    static int i = 0;
+    static int single_regs, add, wback, d, n, imm32, regs;
+    if (type == ARMul_FIRST)
+    {
+        single_regs = BIT(8) == 0;      /* Single precision */
+        add = BIT(23);          /* */
+        wback = BIT(21);        /* write-back */
+        d = single_regs ? BITS(12, 15)<<1|BIT(22) : BIT(22)<<4|BITS(12, 15); /* Base register */
+        n = BITS(16, 19);       /* destination register */
+        imm32 = BITS(0,7) * 4;  /* may not be used */
+        regs = single_regs ? BITS(0, 7) : BITS(0, 7)>>1; /* FSTMX if regs is odd */
+
+        DBG("VSTM :\n");
+
+        if (wback) {
+            state->Reg[n] = (add ? state->Reg[n] + imm32 : state->Reg[n] - imm32);
+            DBG("\twback r%d[%x]\n", n, state->Reg[n]);
+        }
+
+        i = 0;
+
+        return ARMul_DONE;
+    }
+    else if (type == ARMul_DATA)
+    {
+        if (single_regs)
+        {
+            *value = state->ExtReg[d + i];
+            DBG("\taddr[?] <= s%d=[%x]\n", d+i, state->ExtReg[d + i]);
+            i++;
+            if (i < regs)
+                return ARMul_INC;
+            else
+                return ARMul_DONE;
+        }
+        else
+        {
+            /* FIXME Careful of endianness, may need to rework this */
+            *value = state->ExtReg[d*2 + i];
+            DBG("\taddr[?] <= s[%d]=[%x]\n", d*2 + i, state->ExtReg[d*2 + i]);
+            i++;
+            if (i < regs*2)
+                return ARMul_INC;
+            else
+                return ARMul_DONE;
+        }
+    }
+
+    return -1;
+}
 
 /* ----------- LDC ------------ */
-#define VFP_LDC_IMPL
-#include "core/arm/skyeye_common/vfp/vfpinstr.cpp"
-#undef VFP_LDC_IMPL
+int VPOP(ARMul_State* state, int type, ARMword instr, ARMword value)
+{
+    static int i = 0;
+    static int single_regs, add, wback, d, n, imm32, regs;
+    if (type == ARMul_FIRST)
+    {
+        single_regs = BIT(8) == 0;      /* Single precision */
+        d = single_regs ? BITS(12, 15)<<1|BIT(22) : BIT(22)<<4|BITS(12, 15); /* Base register */
+        imm32 = BITS(0,7)<<2;   /* may not be used */
+        regs = single_regs ? BITS(0, 7) : BITS(1, 7); /* FLDMX if regs is odd */
 
+        DBG("VPOP :\n");
+        DBG("\tsp[%x]", state->Reg[R13]);
+        state->Reg[R13] = state->Reg[R13] + imm32;
+        DBG("=>[%x]\n", state->Reg[R13]);
+
+        i = 0;
+
+        return ARMul_DONE;
+    }
+    else if (type == ARMul_TRANSFER)
+    {
+        return ARMul_DONE;
+    }
+    else if (type == ARMul_DATA)
+    {
+        if (single_regs)
+        {
+            state->ExtReg[d + i] = value;
+            DBG("\ts%d <= [%x]\n", d + i, value);
+            i++;
+            if (i < regs)
+                return ARMul_INC;
+            else
+                return ARMul_DONE;
+        }
+        else
+        {
+            /* FIXME Careful of endianness, may need to rework this */
+            state->ExtReg[d*2 + i] = value;
+            DBG("\ts%d <= [%x]\n", d*2 + i, value);
+            i++;
+            if (i < regs*2)
+                return ARMul_INC;
+            else
+                return ARMul_DONE;
+        }
+    }
+
+    return -1;
+}
+int VLDR(ARMul_State* state, int type, ARMword instr, ARMword value)
+{
+    static int i = 0;
+    static int single_reg, add, d, n, imm32, regs;
+    if (type == ARMul_FIRST)
+    {
+        single_reg = BIT(8) == 0;       /* Double precision */
+        add = BIT(23);          /* */
+        imm32 = BITS(0,7)<<2;   /* may not be used */
+        d = single_reg ? BITS(12, 15)<<1|BIT(22) : BIT(22)<<4|BITS(12, 15); /* Base register */
+        n = BITS(16, 19);       /* destination register */
+
+        DBG("VLDR :\n");
+
+        i = 0;
+        regs = 1;
+        
+        return ARMul_DONE;
+    }
+    else if (type == ARMul_TRANSFER)
+    {
+        return ARMul_DONE;
+    }
+    else if (type == ARMul_DATA)
+    {
+        if (single_reg)
+        {
+            state->ExtReg[d+i] = value;
+            DBG("\ts%d <= [%x]\n", d+i, value);
+            i++;
+            if (i < regs)
+                return ARMul_INC;
+            else
+                return ARMul_DONE;
+        }
+        else
+        {
+            /* FIXME Careful of endianness, may need to rework this */
+            state->ExtReg[d*2+i] = value;
+            DBG("\ts[%d] <= [%x]\n", d*2+i, value);
+            i++;
+            if (i < regs*2)
+                return ARMul_INC;
+            else
+                return ARMul_DONE;
+        }
+    }
+
+    return -1;
+}
+int VLDM(ARMul_State* state, int type, ARMword instr, ARMword value)
+{
+    static int i = 0;
+    static int single_regs, add, wback, d, n, imm32, regs;
+    if (type == ARMul_FIRST)
+    {
+        single_regs = BIT(8) == 0;      /* Single precision */
+        add = BIT(23);          /* */
+        wback = BIT(21);        /* write-back */
+        d = single_regs ? BITS(12, 15)<<1|BIT(22) : BIT(22)<<4|BITS(12, 15); /* Base register */
+        n = BITS(16, 19);       /* destination register */
+        imm32 = BITS(0,7) * 4;  /* may not be used */
+        regs = single_regs ? BITS(0, 7) : BITS(0, 7)>>1; /* FLDMX if regs is odd */
+        
+        DBG("VLDM :\n");
+        
+        if (wback) {
+            state->Reg[n] = (add ? state->Reg[n] + imm32 : state->Reg[n] - imm32);
+            DBG("\twback r%d[%x]\n", n, state->Reg[n]);
+        }
+
+        i = 0;
+
+        return ARMul_DONE;
+    }
+    else if (type == ARMul_DATA)
+    {
+        if (single_regs)
+        {
+            state->ExtReg[d + i] = value;
+            DBG("\ts%d <= [%x] addr[?]\n", d+i, state->ExtReg[d + i]);
+            i++;
+            if (i < regs)
+                return ARMul_INC;
+            else
+                return ARMul_DONE;
+        }
+        else
+        {
+            /* FIXME Careful of endianness, may need to rework this */
+            state->ExtReg[d*2 + i] = value;
+            DBG("\ts[%d] <= [%x] addr[?]\n", d*2 + i, state->ExtReg[d*2 + i]);
+            i++;
+            if (i < regs*2)
+                return ARMul_INC;
+            else
+                return ARMul_DONE;
+        }
+    }
+        
+    return -1;
+}
 
 /* ----------- CDP ------------ */
-#define VFP_CDP_IMPL
-#include "core/arm/skyeye_common/vfp/vfpinstr.cpp"
-#undef VFP_CDP_IMPL
+void VMOVI(ARMul_State* state, ARMword single, ARMword d, ARMword imm)
+{
+    DBG("VMOV(I) :\n");
+
+    if (single)
+    {
+        DBG("\ts%d <= [%x]\n", d, imm);
+        state->ExtReg[d] = imm;
+    }
+    else
+    {
+        /* Check endian please */
+        DBG("\ts[%d-%d] <= [%x-%x]\n", d*2+1, d*2, imm, 0);
+        state->ExtReg[d*2+1] = imm;
+        state->ExtReg[d*2] = 0;
+    }
+}
+void VMOVR(ARMul_State* state, ARMword single, ARMword d, ARMword m)
+{
+    DBG("VMOV(R) :\n");
+
+    if (single)
+    {
+        DBG("\ts%d <= s%d[%x]\n", d, m, state->ExtReg[m]);
+        state->ExtReg[d] = state->ExtReg[m];
+    }
+    else
+    {
+        /* Check endian please */
+        DBG("\ts[%d-%d] <= s[%d-%d][%x-%x]\n", d*2+1, d*2, m*2+1, m*2, state->ExtReg[m*2+1], state->ExtReg[m*2]);
+        state->ExtReg[d*2+1] = state->ExtReg[m*2+1];
+        state->ExtReg[d*2] = state->ExtReg[m*2];
+    }
+}
 
 /* Miscellaneous functions */
 int32_t vfp_get_float(arm_core_t* state, unsigned int reg)
@@ -366,8 +893,6 @@ void vfp_put_double(arm_core_t* state, uint64_t val, unsigned int reg)
     state->ExtReg[reg*2+1] = (uint32_t) (val>>32);
 }
 
-
-
 /*
  * Process bitmask of exception conditions. (from vfpmodule.c)
  */
diff --git a/src/core/arm/skyeye_common/vfp/vfp.h b/src/core/arm/skyeye_common/vfp/vfp.h
index 7256701f3..f9e8d521d 100644
--- a/src/core/arm/skyeye_common/vfp/vfp.h
+++ b/src/core/arm/skyeye_common/vfp/vfp.h
@@ -27,6 +27,12 @@
 
 #include "core/arm/skyeye_common/vfp/vfp_helper.h" /* for references to cdp SoftFloat functions */
 
+#define VFP_DEBUG_TRANSLATE DBG("in func %s, %x\n", __FUNCTION__, inst);
+#define VFP_DEBUG_UNIMPLEMENTED(x) printf("in func %s, " #x " unimplemented\n", __FUNCTION__); exit(-1);
+#define VFP_DEBUG_UNTESTED(x) printf("in func %s, " #x " untested\n", __FUNCTION__);
+#define CHECK_VFP_ENABLED
+#define CHECK_VFP_CDP_RET       vfp_raise_exceptions(cpu, ret, inst_cream->instr, cpu->VFP[VFP_OFFSET(VFP_FPSCR)]); //if (ret == -1) {printf("VFP CDP FAILURE %x\n", inst_cream->instr); exit(-1);}
+
 unsigned VFPInit (ARMul_State *state);
 unsigned VFPMRC (ARMul_State * state, unsigned type, ARMword instr, ARMword * value);
 unsigned VFPMCR (ARMul_State * state, unsigned type, ARMword instr, ARMword value);
diff --git a/src/core/arm/skyeye_common/vfp/vfpinstr.cpp b/src/core/arm/skyeye_common/vfp/vfpinstr.cpp
index 45208fb13..27dc8a008 100644
--- a/src/core/arm/skyeye_common/vfp/vfpinstr.cpp
+++ b/src/core/arm/skyeye_common/vfp/vfpinstr.cpp
@@ -28,34 +28,19 @@
 /* ----------------------------------------------------------------------- */
 /* VMLA */
 /* cond 1110 0D00 Vn-- Vd-- 101X N0M0 Vm-- */
-#define vfpinstr        vmla
-#define vfpinstr_inst   vmla_inst
-#define VFPLABEL_INST   VMLA_INST
-#ifdef VFP_DECODE
-{"vmla",        4,      ARMVFP2,        23, 27, 0x1c,   20, 21, 0x0,    9, 11, 0x5,     4, 4, 0},
-#endif
-#ifdef VFP_DECODE_EXCLUSION
-{"vmla",        0,      ARMVFP2, 0},
-#endif
-#ifdef VFP_INTERPRETER_TABLE
-INTERPRETER_TRANSLATE(vfpinstr),
-#endif
-#ifdef VFP_INTERPRETER_LABEL
-&&VFPLABEL_INST,
-#endif
 #ifdef VFP_INTERPRETER_STRUCT
 typedef struct _vmla_inst {
         unsigned int instr;
         unsigned int dp_operation;
-} vfpinstr_inst;
+} vmla_inst;
 #endif
 #ifdef VFP_INTERPRETER_TRANS
-ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
+ARM_INST_PTR INTERPRETER_TRANSLATE(vmla)(unsigned int inst, int index)
 {
         VFP_DEBUG_TRANSLATE;
         
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vfpinstr_inst));
-        vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vmla_inst));
+        vmla_inst *inst_cream = (vmla_inst *)inst_base->component;
 
         inst_base->cond  = BITS(inst, 28, 31);
         inst_base->idx   = index;
@@ -69,15 +54,14 @@ ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
 }
 #endif
 #ifdef VFP_INTERPRETER_IMPL
-VFPLABEL_INST:
+VMLA_INST:
 {
-        INC_ICOUNTER;
         if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
                 CHECK_VFP_ENABLED;
                 
                 DBG("VMLA :\n");
                 
-                vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+                vmla_inst *inst_cream = (vmla_inst *)inst_base->component;
 
                 int ret;
                 
@@ -89,22 +73,17 @@ VFPLABEL_INST:
                 CHECK_VFP_CDP_RET;
         }
         cpu->Reg[15] += GET_INST_SIZE(cpu);
-        INC_PC(sizeof(vfpinstr_inst));
+        INC_PC(sizeof(vmla_inst));
         FETCH_INST;
         GOTO_NEXT_INST;
 }
 #endif
-#ifdef VFP_CDP_TRANS
-if ((OPC_1 & 0xB) == 0 && (OPC_2 & 0x2) == 0)
-{
-        DBG("VMLA :\n");
-}
-#endif
+
 #ifdef VFP_DYNCOM_TABLE
-DYNCOM_FILL_ACTION(vfpinstr),
+DYNCOM_FILL_ACTION(vmla),
 #endif
 #ifdef VFP_DYNCOM_TAG
-int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
+int DYNCOM_TAG(vmla)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
 {
         int instr_size = INSTR_SIZE;
         //DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
@@ -114,7 +93,7 @@ int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr
 }
 #endif
 #ifdef VFP_DYNCOM_TRANS
-int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
+int DYNCOM_TRANS(vmla)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
         //DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
         //arch_arm_undef(cpu, bb, instr);
         int m;
@@ -168,41 +147,23 @@ int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc
         return No_exp;
 }
 #endif
-#undef vfpinstr
-#undef vfpinstr_inst
-#undef VFPLABEL_INST
 
 /* ----------------------------------------------------------------------- */
 /* VNMLS */
 /* cond 1110 0D00 Vn-- Vd-- 101X N1M0 Vm-- */
-#define vfpinstr        vmls
-#define vfpinstr_inst   vmls_inst
-#define VFPLABEL_INST   VMLS_INST
-#ifdef VFP_DECODE
-{"vmls",        7,      ARMVFP2,    28 , 31, 0xF, 25, 27, 0x1,   23, 23, 1,  11, 11, 0,  8, 9, 0x2,  6, 6, 1,  4, 4, 0},
-#endif
-#ifdef VFP_DECODE_EXCLUSION
-{"vmls",        0,      ARMVFP2, 0},
-#endif
-#ifdef VFP_INTERPRETER_TABLE
-INTERPRETER_TRANSLATE(vfpinstr),
-#endif
-#ifdef VFP_INTERPRETER_LABEL
-&&VFPLABEL_INST,
-#endif
 #ifdef VFP_INTERPRETER_STRUCT
 typedef struct _vmls_inst {
         unsigned int instr;
         unsigned int dp_operation;
-} vfpinstr_inst;
+} vmls_inst;
 #endif
 #ifdef VFP_INTERPRETER_TRANS
-ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
+ARM_INST_PTR INTERPRETER_TRANSLATE(vmls)(unsigned int inst, int index)
 {
         VFP_DEBUG_TRANSLATE;
         
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vfpinstr_inst));
-        vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vmls_inst));
+        vmls_inst *inst_cream = (vmls_inst *)inst_base->component;
 
         inst_base->cond  = BITS(inst, 28, 31);
         inst_base->idx   = index;
@@ -216,15 +177,14 @@ ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
 }
 #endif
 #ifdef VFP_INTERPRETER_IMPL
-VFPLABEL_INST:
+VMLS_INST:
 {
-        INC_ICOUNTER;
         if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
                 CHECK_VFP_ENABLED;
                 
                 DBG("VMLS :\n");
                 
-                vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+                vmls_inst *inst_cream = (vmls_inst *)inst_base->component;
 
                 int ret;
                 
@@ -236,22 +196,17 @@ VFPLABEL_INST:
                 CHECK_VFP_CDP_RET;
         }
         cpu->Reg[15] += GET_INST_SIZE(cpu);
-        INC_PC(sizeof(vfpinstr_inst));
+        INC_PC(sizeof(vmls_inst));
         FETCH_INST;
         GOTO_NEXT_INST;
 }
 #endif
-#ifdef VFP_CDP_TRANS
-if ((OPC_1 & 0xB) == 0 && (OPC_2 & 0x2) == 2)
-{
-        DBG("VMLS :\n");
-}
-#endif
+
 #ifdef VFP_DYNCOM_TABLE
-DYNCOM_FILL_ACTION(vfpinstr),
+DYNCOM_FILL_ACTION(vmls),
 #endif
 #ifdef VFP_DYNCOM_TAG
-int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
+int DYNCOM_TAG(vmls)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
 {
         int instr_size = INSTR_SIZE;
         //DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
@@ -261,7 +216,7 @@ int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr
 }
 #endif
 #ifdef VFP_DYNCOM_TRANS
-int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
+int DYNCOM_TRANS(vmls)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
         DBG("\t\tin %s VMLS instruction is executed out of here.\n", __FUNCTION__);
         //arch_arm_undef(cpu, bb, instr);
         int m;
@@ -315,47 +270,23 @@ int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc
         return No_exp;
 }
 #endif
-#undef vfpinstr
-#undef vfpinstr_inst
-#undef VFPLABEL_INST
 
 /* ----------------------------------------------------------------------- */
 /* VNMLA */
 /* cond 1110 0D01 Vn-- Vd-- 101X N1M0 Vm-- */
-#define vfpinstr        vnmla
-#define vfpinstr_inst   vnmla_inst
-#define VFPLABEL_INST   VNMLA_INST
-#ifdef VFP_DECODE
-//{"vnmla",       5,      ARMVFP2,        23, 27, 0x1c,   20, 21, 0x0,    9, 11, 0x5,     6, 6, 1,        4, 4, 0},
-{"vnmla",       4,      ARMVFP2,        23, 27, 0x1c,   20, 21, 0x1,    9, 11, 0x5,     4, 4, 0},
-{"vnmla",       5,      ARMVFP2,        23, 27, 0x1c,   20, 21, 0x2,    9, 11, 0x5,     6, 6, 1,  4, 4, 0},
-//{"vnmla",       5,      ARMVFP2,        23, 27, 0x1c,   20, 21, 0x2,    9, 11, 0x5,     6, 6, 1,        4, 4, 0},
-#endif
-#ifdef VFP_DECODE_EXCLUSION
-{"vnmla",       0,      ARMVFP2, 0},
-{"vnmla",       0,      ARMVFP2, 0},
-#endif
-#ifdef VFP_INTERPRETER_TABLE
-INTERPRETER_TRANSLATE(vfpinstr),
-INTERPRETER_TRANSLATE(vfpinstr),
-#endif
-#ifdef VFP_INTERPRETER_LABEL
-&&VFPLABEL_INST,
-&&VFPLABEL_INST,
-#endif
 #ifdef VFP_INTERPRETER_STRUCT
 typedef struct _vnmla_inst {
         unsigned int instr;
         unsigned int dp_operation;
-} vfpinstr_inst;
+} vnmla_inst;
 #endif
 #ifdef VFP_INTERPRETER_TRANS
-ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
+ARM_INST_PTR INTERPRETER_TRANSLATE(vnmla)(unsigned int inst, int index)
 {
         VFP_DEBUG_TRANSLATE;
         
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vfpinstr_inst));
-        vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vnmla_inst));
+        vnmla_inst *inst_cream = (vnmla_inst *)inst_base->component;
 
         inst_base->cond  = BITS(inst, 28, 31);
         inst_base->idx   = index;
@@ -369,15 +300,14 @@ ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
 }
 #endif
 #ifdef VFP_INTERPRETER_IMPL
-VFPLABEL_INST:
+VNMLA_INST:
 {
-        INC_ICOUNTER;
         if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
                 CHECK_VFP_ENABLED;
                 
                 DBG("VNMLA :\n");
 
-                vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+                vnmla_inst *inst_cream = (vnmla_inst *)inst_base->component;
 
                 int ret;
                 
@@ -389,23 +319,18 @@ VFPLABEL_INST:
                 CHECK_VFP_CDP_RET;
         }
         cpu->Reg[15] += GET_INST_SIZE(cpu);
-        INC_PC(sizeof(vfpinstr_inst));
+        INC_PC(sizeof(vnmla_inst));
         FETCH_INST;
         GOTO_NEXT_INST;
 }
 #endif
-#ifdef VFP_CDP_TRANS
-if ((OPC_1 & 0xB) == 1 && (OPC_2 & 0x2) == 2)
-{
-        DBG("VNMLA :\n");
-}
-#endif
+
 #ifdef VFP_DYNCOM_TABLE
-DYNCOM_FILL_ACTION(vfpinstr),
-DYNCOM_FILL_ACTION(vfpinstr),
+DYNCOM_FILL_ACTION(vnmla),
+DYNCOM_FILL_ACTION(vnmla),
 #endif
 #ifdef VFP_DYNCOM_TAG
-int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
+int DYNCOM_TAG(vnmla)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
 {
         int instr_size = INSTR_SIZE;
         //DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
@@ -415,7 +340,7 @@ int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr
 }
 #endif
 #ifdef VFP_DYNCOM_TRANS
-int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
+int DYNCOM_TRANS(vnmla)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
         DBG("\t\tin %s VNMLA instruction is executed out of here.\n", __FUNCTION__);
         //arch_arm_undef(cpu, bb, instr);
         int m;
@@ -469,41 +394,24 @@ int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc
         return No_exp;
 }
 #endif
-#undef vfpinstr
-#undef vfpinstr_inst
-#undef VFPLABEL_INST
 
 /* ----------------------------------------------------------------------- */
 /* VNMLS */
 /* cond 1110 0D01 Vn-- Vd-- 101X N0M0 Vm-- */
-#define vfpinstr        vnmls
-#define vfpinstr_inst   vnmls_inst
-#define VFPLABEL_INST   VNMLS_INST
-#ifdef VFP_DECODE
-{"vnmls",       5,      ARMVFP2,        23, 27, 0x1c,   20, 21, 0x1,    9, 11, 0x5,     6, 6, 0,        4, 4, 0},
-#endif
-#ifdef VFP_DECODE_EXCLUSION
-{"vnmls",       0,      ARMVFP2, 0},
-#endif
-#ifdef VFP_INTERPRETER_TABLE
-INTERPRETER_TRANSLATE(vfpinstr),
-#endif
-#ifdef VFP_INTERPRETER_LABEL
-&&VFPLABEL_INST,
-#endif
+
 #ifdef VFP_INTERPRETER_STRUCT
 typedef struct _vnmls_inst {
         unsigned int instr;
         unsigned int dp_operation;
-} vfpinstr_inst;
+} vnmls_inst;
 #endif
 #ifdef VFP_INTERPRETER_TRANS
-ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
+ARM_INST_PTR INTERPRETER_TRANSLATE(vnmls)(unsigned int inst, int index)
 {
         VFP_DEBUG_TRANSLATE;
         
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vfpinstr_inst));
-        vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vnmls_inst));
+        vnmls_inst *inst_cream = (vnmls_inst *)inst_base->component;
 
         inst_base->cond  = BITS(inst, 28, 31);
         inst_base->idx   = index;
@@ -517,15 +425,14 @@ ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
 }
 #endif
 #ifdef VFP_INTERPRETER_IMPL
-VFPLABEL_INST:
+VNMLS_INST:
 {
-        INC_ICOUNTER;
         if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
                 CHECK_VFP_ENABLED;
                 
                 DBG("VNMLS :\n");
 
-                vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+                vnmls_inst *inst_cream = (vnmls_inst *)inst_base->component;
 
                 int ret;
                 
@@ -537,22 +444,17 @@ VFPLABEL_INST:
                 CHECK_VFP_CDP_RET;
         }
         cpu->Reg[15] += GET_INST_SIZE(cpu);
-        INC_PC(sizeof(vfpinstr_inst));
+        INC_PC(sizeof(vnmls_inst));
         FETCH_INST;
         GOTO_NEXT_INST;
 }
 #endif
-#ifdef VFP_CDP_TRANS
-if ((OPC_1 & 0xB) == 1 && (OPC_2 & 0x2) == 0)
-{
-        DBG("VNMLS :\n");
-}
-#endif
+
 #ifdef VFP_DYNCOM_TABLE
-DYNCOM_FILL_ACTION(vfpinstr),
+DYNCOM_FILL_ACTION(vnmls),
 #endif
 #ifdef VFP_DYNCOM_TAG
-int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
+int DYNCOM_TAG(vnmls)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
 {
         int instr_size = INSTR_SIZE;
         DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
@@ -562,7 +464,7 @@ int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr
 }
 #endif
 #ifdef VFP_DYNCOM_TRANS
-int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
+int DYNCOM_TRANS(vnmls)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
         DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
         //arch_arm_undef(cpu, bb, instr);
         int m;
@@ -616,41 +518,23 @@ int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc
         return No_exp;
 }
 #endif
-#undef vfpinstr
-#undef vfpinstr_inst
-#undef VFPLABEL_INST
 
 /* ----------------------------------------------------------------------- */
 /* VNMUL */
 /* cond 1110 0D10 Vn-- Vd-- 101X N0M0 Vm-- */
-#define vfpinstr        vnmul
-#define vfpinstr_inst   vnmul_inst
-#define VFPLABEL_INST   VNMUL_INST
-#ifdef VFP_DECODE
-{"vnmul",       5,      ARMVFP2,        23, 27, 0x1c,   20, 21, 0x2,    9, 11, 0x5,     6, 6, 1,        4, 4, 0},
-#endif
-#ifdef VFP_DECODE_EXCLUSION
-{"vnmul",       0,      ARMVFP2, 0},
-#endif
-#ifdef VFP_INTERPRETER_TABLE
-INTERPRETER_TRANSLATE(vfpinstr),
-#endif
-#ifdef VFP_INTERPRETER_LABEL
-&&VFPLABEL_INST,
-#endif
 #ifdef VFP_INTERPRETER_STRUCT
 typedef struct _vnmul_inst {
         unsigned int instr;
         unsigned int dp_operation;
-} vfpinstr_inst;
+} vnmul_inst;
 #endif
 #ifdef VFP_INTERPRETER_TRANS
-ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
+ARM_INST_PTR INTERPRETER_TRANSLATE(vnmul)(unsigned int inst, int index)
 {
         VFP_DEBUG_TRANSLATE;
         
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vfpinstr_inst));
-        vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vnmul_inst));
+        vnmul_inst *inst_cream = (vnmul_inst *)inst_base->component;
 
         inst_base->cond  = BITS(inst, 28, 31);
         inst_base->idx   = index;
@@ -664,15 +548,14 @@ ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
 }
 #endif
 #ifdef VFP_INTERPRETER_IMPL
-VFPLABEL_INST:
+VNMUL_INST:
 {
-        INC_ICOUNTER;
         if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
                 CHECK_VFP_ENABLED;
                 
                 DBG("VNMUL :\n");
 
-                vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+                vnmul_inst *inst_cream = (vnmul_inst *)inst_base->component;
 
                 int ret;
                 
@@ -684,22 +567,17 @@ VFPLABEL_INST:
                 CHECK_VFP_CDP_RET;
         }
         cpu->Reg[15] += GET_INST_SIZE(cpu);
-        INC_PC(sizeof(vfpinstr_inst));
+        INC_PC(sizeof(vnmul_inst));
         FETCH_INST;
         GOTO_NEXT_INST;
 }
 #endif
-#ifdef VFP_CDP_TRANS
-if ((OPC_1 & 0xB) == 2 && (OPC_2 & 0x2) == 2)
-{
-        DBG("VNMUL :\n");
-}
-#endif
+
 #ifdef VFP_DYNCOM_TABLE
-DYNCOM_FILL_ACTION(vfpinstr),
+DYNCOM_FILL_ACTION(vnmul),
 #endif
 #ifdef VFP_DYNCOM_TAG
-int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
+int DYNCOM_TAG(vnmul)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
 {
         int instr_size = INSTR_SIZE;
         DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
@@ -709,7 +587,7 @@ int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr
 }               
 #endif
 #ifdef VFP_DYNCOM_TRANS
-int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
+int DYNCOM_TRANS(vnmul)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
         DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
         //arch_arm_undef(cpu, bb, instr);
         int m;
@@ -753,41 +631,24 @@ int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc
         return No_exp;
 }
 #endif
-#undef vfpinstr
-#undef vfpinstr_inst
-#undef VFPLABEL_INST
+
 
 /* ----------------------------------------------------------------------- */
 /* VMUL */
 /* cond 1110 0D10 Vn-- Vd-- 101X N0M0 Vm-- */
-#define vfpinstr        vmul
-#define vfpinstr_inst   vmul_inst
-#define VFPLABEL_INST   VMUL_INST
-#ifdef VFP_DECODE
-{"vmul",        5,      ARMVFP2,        23, 27, 0x1c,  20, 21, 0x2,     9, 11, 0x5,      6, 6, 0,     4, 4, 0},
-#endif
-#ifdef VFP_DECODE_EXCLUSION
-{"vmul",        0,      ARMVFP2, 0},
-#endif
-#ifdef VFP_INTERPRETER_TABLE
-INTERPRETER_TRANSLATE(vfpinstr),
-#endif
-#ifdef VFP_INTERPRETER_LABEL
-&&VFPLABEL_INST,
-#endif
 #ifdef VFP_INTERPRETER_STRUCT
 typedef struct _vmul_inst {
         unsigned int instr;
         unsigned int dp_operation;
-} vfpinstr_inst;
+} vmul_inst;
 #endif
 #ifdef VFP_INTERPRETER_TRANS
-ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
+ARM_INST_PTR INTERPRETER_TRANSLATE(vmul)(unsigned int inst, int index)
 {
         VFP_DEBUG_TRANSLATE;
         
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vfpinstr_inst));
-        vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vmul_inst));
+        vmul_inst *inst_cream = (vmul_inst *)inst_base->component;
 
         inst_base->cond  = BITS(inst, 28, 31);
         inst_base->idx   = index;
@@ -801,15 +662,14 @@ ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
 }
 #endif
 #ifdef VFP_INTERPRETER_IMPL
-VFPLABEL_INST:
+VMUL_INST:
 {
-        INC_ICOUNTER;
         if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
                 CHECK_VFP_ENABLED;
                 
                 DBG("VMUL :\n");
 
-                vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+                vmul_inst *inst_cream = (vmul_inst *)inst_base->component;
 
                 int ret;
                 
@@ -821,22 +681,17 @@ VFPLABEL_INST:
                 CHECK_VFP_CDP_RET;
         }
         cpu->Reg[15] += GET_INST_SIZE(cpu);
-        INC_PC(sizeof(vfpinstr_inst));
+        INC_PC(sizeof(vmul_inst));
         FETCH_INST;
         GOTO_NEXT_INST;
 }
 #endif
-#ifdef VFP_CDP_TRANS
-if ((OPC_1 & 0xB) == 2 && (OPC_2 & 0x2) == 0)
-{
-        DBG("VMUL :\n");
-}
-#endif
+
 #ifdef VFP_DYNCOM_TABLE
-DYNCOM_FILL_ACTION(vfpinstr),
+DYNCOM_FILL_ACTION(vmul),
 #endif
 #ifdef VFP_DYNCOM_TAG
-int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
+int DYNCOM_TAG(vmul)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
 {
         int instr_size = INSTR_SIZE;
         //DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
@@ -846,7 +701,7 @@ int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr
 }
 #endif
 #ifdef VFP_DYNCOM_TRANS
-int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
+int DYNCOM_TRANS(vmul)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
         DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
         //printf("\n\n\t\tin %s instruction is executed out.\n\n", __FUNCTION__);
         //arch_arm_undef(cpu, bb, instr);
@@ -904,41 +759,23 @@ int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc
         return No_exp;
 }
 #endif
-#undef vfpinstr
-#undef vfpinstr_inst
-#undef VFPLABEL_INST
 
 /* ----------------------------------------------------------------------- */
 /* VADD */
 /* cond 1110 0D11 Vn-- Vd-- 101X N0M0 Vm-- */
-#define vfpinstr        vadd
-#define vfpinstr_inst   vadd_inst
-#define VFPLABEL_INST   VADD_INST
-#ifdef VFP_DECODE
-{"vadd",        5,      ARMVFP2,        23, 27, 0x1c,  20, 21, 0x3,     9, 11, 0x5,      6, 6, 0,     4, 4, 0},
-#endif
-#ifdef VFP_DECODE_EXCLUSION
-{"vadd",        0,      ARMVFP2, 0},
-#endif
-#ifdef VFP_INTERPRETER_TABLE
-INTERPRETER_TRANSLATE(vfpinstr),
-#endif
-#ifdef VFP_INTERPRETER_LABEL
-&&VFPLABEL_INST,
-#endif
 #ifdef VFP_INTERPRETER_STRUCT
 typedef struct _vadd_inst {
         unsigned int instr;
         unsigned int dp_operation;
-} vfpinstr_inst;
+} vadd_inst;
 #endif
 #ifdef VFP_INTERPRETER_TRANS
-ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
+ARM_INST_PTR INTERPRETER_TRANSLATE(vadd)(unsigned int inst, int index)
 {
         VFP_DEBUG_TRANSLATE;
         
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vfpinstr_inst));
-        vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vadd_inst));
+        vadd_inst *inst_cream = (vadd_inst *)inst_base->component;
 
         inst_base->cond  = BITS(inst, 28, 31);
         inst_base->idx   = index;
@@ -952,15 +789,14 @@ ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
 }
 #endif
 #ifdef VFP_INTERPRETER_IMPL
-VFPLABEL_INST:
+VADD_INST:
 {
-        INC_ICOUNTER;
         if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
                 CHECK_VFP_ENABLED;
                 
                 DBG("VADD :\n");
 
-                vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+                vadd_inst *inst_cream = (vadd_inst *)inst_base->component;
 
                 int ret;
                 
@@ -972,22 +808,17 @@ VFPLABEL_INST:
                 CHECK_VFP_CDP_RET;
         }
         cpu->Reg[15] += GET_INST_SIZE(cpu);
-        INC_PC(sizeof(vfpinstr_inst));
+        INC_PC(sizeof(vadd_inst));
         FETCH_INST;
         GOTO_NEXT_INST;
 }
 #endif
-#ifdef VFP_CDP_TRANS
-if ((OPC_1 & 0xB) == 3 && (OPC_2 & 0x2) == 0)
-{
-        DBG("VADD :\n");
-}
-#endif
+
 #ifdef VFP_DYNCOM_TABLE
-DYNCOM_FILL_ACTION(vfpinstr),
+DYNCOM_FILL_ACTION(vadd),
 #endif
 #ifdef VFP_DYNCOM_TAG
-int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
+int DYNCOM_TAG(vadd)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
 {
         int instr_size = INSTR_SIZE;
         DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
@@ -997,7 +828,7 @@ int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr
 }
 #endif
 #ifdef VFP_DYNCOM_TRANS
-int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
+int DYNCOM_TRANS(vadd)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
         DBG("\t\tin %s instruction will implement out of JIT.\n", __FUNCTION__);
         //arch_arm_undef(cpu, bb, instr);
         int m;
@@ -1049,41 +880,23 @@ int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc
         return No_exp;
 }
 #endif
-#undef vfpinstr
-#undef vfpinstr_inst
-#undef VFPLABEL_INST
 
 /* ----------------------------------------------------------------------- */
 /* VSUB */
 /* cond 1110 0D11 Vn-- Vd-- 101X N1M0 Vm-- */
-#define vfpinstr        vsub
-#define vfpinstr_inst   vsub_inst
-#define VFPLABEL_INST   VSUB_INST
-#ifdef VFP_DECODE
-{"vsub",        5,      ARMVFP2,        23, 27, 0x1c,  20, 21, 0x3,     9, 11, 0x5,      6, 6, 1,     4, 4, 0},
-#endif
-#ifdef VFP_DECODE_EXCLUSION
-{"vsub",        0,      ARMVFP2, 0},
-#endif
-#ifdef VFP_INTERPRETER_TABLE
-INTERPRETER_TRANSLATE(vfpinstr),
-#endif
-#ifdef VFP_INTERPRETER_LABEL
-&&VFPLABEL_INST,
-#endif
 #ifdef VFP_INTERPRETER_STRUCT
 typedef struct _vsub_inst {
         unsigned int instr;
         unsigned int dp_operation;
-} vfpinstr_inst;
+} vsub_inst;
 #endif
 #ifdef VFP_INTERPRETER_TRANS
-ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
+ARM_INST_PTR INTERPRETER_TRANSLATE(vsub)(unsigned int inst, int index)
 {
         VFP_DEBUG_TRANSLATE;
         
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vfpinstr_inst));
-        vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vsub_inst));
+        vsub_inst *inst_cream = (vsub_inst *)inst_base->component;
 
         inst_base->cond  = BITS(inst, 28, 31);
         inst_base->idx   = index;
@@ -1097,15 +910,14 @@ ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
 }
 #endif
 #ifdef VFP_INTERPRETER_IMPL
-VFPLABEL_INST:
+VSUB_INST:
 {
-        INC_ICOUNTER;
         if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
                 CHECK_VFP_ENABLED;
                 
                 DBG("VSUB :\n");
 
-                vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+                vsub_inst *inst_cream = (vsub_inst *)inst_base->component;
 
                 int ret;
                 
@@ -1117,22 +929,16 @@ VFPLABEL_INST:
                 CHECK_VFP_CDP_RET;
         }
         cpu->Reg[15] += GET_INST_SIZE(cpu);
-        INC_PC(sizeof(vfpinstr_inst));
+        INC_PC(sizeof(vsub_inst));
         FETCH_INST;
         GOTO_NEXT_INST;
 }
 #endif
-#ifdef VFP_CDP_TRANS
-if ((OPC_1 & 0xB) == 3 && (OPC_2 & 0x2) == 2)
-{
-        DBG("VSUB :\n");
-}
-#endif
 #ifdef VFP_DYNCOM_TABLE
-DYNCOM_FILL_ACTION(vfpinstr),
+DYNCOM_FILL_ACTION(vsub),
 #endif
 #ifdef VFP_DYNCOM_TAG
-int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
+int DYNCOM_TAG(vsub)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
 {
         int instr_size = INSTR_SIZE;
         //arm_tag_trap(cpu, pc, instr, tag, new_pc, next_pc);
@@ -1141,7 +947,7 @@ int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr
 }
 #endif
 #ifdef VFP_DYNCOM_TRANS
-int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
+int DYNCOM_TRANS(vsub)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
         DBG("\t\tin %s instr=0x%x, instruction is executed out of JIT.\n", __FUNCTION__, instr);
         //arch_arm_undef(cpu, bb, instr);
         int m;
@@ -1193,41 +999,23 @@ int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc
         return No_exp;
 }
 #endif
-#undef vfpinstr
-#undef vfpinstr_inst
-#undef VFPLABEL_INST
 
 /* ----------------------------------------------------------------------- */
 /* VDIV */
 /* cond 1110 1D00 Vn-- Vd-- 101X N0M0 Vm-- */
-#define vfpinstr        vdiv
-#define vfpinstr_inst   vdiv_inst
-#define VFPLABEL_INST   VDIV_INST
-#ifdef VFP_DECODE
-{"vdiv",        5,      ARMVFP2,        23, 27, 0x1d,  20, 21, 0x0,     9, 11, 0x5,      6, 6, 0,     4, 4, 0},
-#endif
-#ifdef VFP_DECODE_EXCLUSION
-{"vdiv",        0,      ARMVFP2, 0},
-#endif
-#ifdef VFP_INTERPRETER_TABLE
-INTERPRETER_TRANSLATE(vfpinstr),
-#endif
-#ifdef VFP_INTERPRETER_LABEL
-&&VFPLABEL_INST,
-#endif
 #ifdef VFP_INTERPRETER_STRUCT
 typedef struct _vdiv_inst {
         unsigned int instr;
         unsigned int dp_operation;
-} vfpinstr_inst;
+} vdiv_inst;
 #endif
 #ifdef VFP_INTERPRETER_TRANS
-ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
+ARM_INST_PTR INTERPRETER_TRANSLATE(vdiv)(unsigned int inst, int index)
 {
         VFP_DEBUG_TRANSLATE;
         
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vfpinstr_inst));
-        vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vdiv_inst));
+        vdiv_inst *inst_cream = (vdiv_inst *)inst_base->component;
 
         inst_base->cond  = BITS(inst, 28, 31);
         inst_base->idx   = index;
@@ -1241,15 +1029,14 @@ ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
 }
 #endif
 #ifdef VFP_INTERPRETER_IMPL
-VFPLABEL_INST:
+VDIV_INST:
 {
-        INC_ICOUNTER;
         if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
                 CHECK_VFP_ENABLED;
                 
                 DBG("VDIV :\n");
 
-                vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+                vdiv_inst *inst_cream = (vdiv_inst *)inst_base->component;
 
                 int ret;
                 
@@ -1261,22 +1048,17 @@ VFPLABEL_INST:
                 CHECK_VFP_CDP_RET;
         }
         cpu->Reg[15] += GET_INST_SIZE(cpu);
-        INC_PC(sizeof(vfpinstr_inst));
+        INC_PC(sizeof(vdiv_inst));
         FETCH_INST;
         GOTO_NEXT_INST;
 }
 #endif
-#ifdef VFP_CDP_TRANS
-if ((OPC_1 & 0xB) == 0xA && (OPC_2 & 0x2) == 0)
-{
-        DBG("VDIV :\n");
-}
-#endif
+
 #ifdef VFP_DYNCOM_TABLE
-DYNCOM_FILL_ACTION(vfpinstr),
+DYNCOM_FILL_ACTION(vdiv),
 #endif
 #ifdef VFP_DYNCOM_TAG
-int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
+int DYNCOM_TAG(vdiv)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
 {
         int instr_size = INSTR_SIZE;
         DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
@@ -1286,7 +1068,7 @@ int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr
 }
 #endif
 #ifdef VFP_DYNCOM_TRANS
-int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
+int DYNCOM_TRANS(vdiv)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
         DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
         //arch_arm_undef(cpu, bb, instr);
         int m;
@@ -1338,43 +1120,25 @@ int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc
         return No_exp;
 }
 #endif
-#undef vfpinstr
-#undef vfpinstr_inst
-#undef VFPLABEL_INST
 
 /* ----------------------------------------------------------------------- */
 /* VMOVI move immediate */
 /* cond 1110 1D11 im4H Vd-- 101X 0000 im4L */
 /* cond 1110 opc1 CRn- CRd- copr op20 CRm- CDP */
-#define vfpinstr        vmovi
-#define vfpinstr_inst   vmovi_inst
-#define VFPLABEL_INST   VMOVI_INST
-#ifdef VFP_DECODE
-{"vmov(i)",       4,      ARMVFP3,        23, 27, 0x1d,   20, 21, 0x3,    9, 11, 0x5,     4, 7, 0},
-#endif
-#ifdef VFP_DECODE_EXCLUSION
-{"vmov(i)",       0,      ARMVFP3, 0},
-#endif
-#ifdef VFP_INTERPRETER_TABLE
-INTERPRETER_TRANSLATE(vfpinstr),
-#endif
-#ifdef VFP_INTERPRETER_LABEL
-&&VFPLABEL_INST,
-#endif
 #ifdef VFP_INTERPRETER_STRUCT
 typedef struct _vmovi_inst {
         unsigned int single;
         unsigned int d;
         unsigned int imm;
-} vfpinstr_inst;
+} vmovi_inst;
 #endif
 #ifdef VFP_INTERPRETER_TRANS
-ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
+ARM_INST_PTR INTERPRETER_TRANSLATE(vmovi)(unsigned int inst, int index)
 {
         VFP_DEBUG_TRANSLATE;
         
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vfpinstr_inst));
-        vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vmovi_inst));
+        vmovi_inst *inst_cream = (vmovi_inst *)inst_base->component;
 
         inst_base->cond  = BITS(inst, 28, 31);
         inst_base->idx   = index;
@@ -1392,62 +1156,27 @@ ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
 }
 #endif
 #ifdef VFP_INTERPRETER_IMPL
-VFPLABEL_INST:
+VMOVI_INST:
 {
-        INC_ICOUNTER;
         if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
                 CHECK_VFP_ENABLED;
                 
-                vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+                vmovi_inst *inst_cream = (vmovi_inst *)inst_base->component;
 
                 VMOVI(cpu, inst_cream->single, inst_cream->d, inst_cream->imm);
         }
         cpu->Reg[15] += GET_INST_SIZE(cpu);
-        INC_PC(sizeof(vfpinstr_inst));
+        INC_PC(sizeof(vmovi_inst));
         FETCH_INST;
         GOTO_NEXT_INST;
 }
 #endif
-#ifdef VFP_CDP_TRANS
-if ( (OPC_1 & 0xb) == 0xb && BITS(4, 7) == 0)
-{
-        unsigned int single   = BIT(8) == 0;
-        unsigned int d        = (single ? BITS(12,15)<<1 | BIT(22) : BITS(12,15) | BIT(22)<<4);
-        unsigned int imm;
-        instr = BITS(16, 19) << 4 | BITS(0, 3); /* FIXME dirty workaround to get a correct imm */
-        if (single) {
-                imm = BIT(7)<<31 | (BIT(6)==0)<<30 | (BIT(6) ? 0x1f : 0)<<25 | BITS(0, 5)<<19;
-        } else {
-                imm = BIT(7)<<31 | (BIT(6)==0)<<30 | (BIT(6) ? 0xff : 0)<<22 | BITS(0, 5)<<16;
-        }
-        VMOVI(state, single, d, imm);
-        return ARMul_DONE;
-}
-#endif
-#ifdef VFP_CDP_IMPL
-void VMOVI(ARMul_State * state, ARMword single, ARMword d, ARMword imm)
-{
-        DBG("VMOV(I) :\n");
-                
-        if (single)
-        {
-                DBG("\ts%d <= [%x]\n", d, imm);
-                state->ExtReg[d] = imm;
-        }
-        else
-        {
-                /* Check endian please */
-                DBG("\ts[%d-%d] <= [%x-%x]\n", d*2+1, d*2, imm, 0);
-                state->ExtReg[d*2+1] = imm;
-                state->ExtReg[d*2] = 0;
-        }
-}
-#endif
+
 #ifdef VFP_DYNCOM_TABLE
-DYNCOM_FILL_ACTION(vfpinstr),
+DYNCOM_FILL_ACTION(vmovi),
 #endif
 #ifdef VFP_DYNCOM_TAG
-int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
+int DYNCOM_TAG(vmovi)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
 {
         int instr_size = INSTR_SIZE;
         DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
@@ -1456,7 +1185,7 @@ int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr
 }
 #endif
 #ifdef VFP_DYNCOM_TRANS
-int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
+int DYNCOM_TRANS(vmovi)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
         DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
         //arch_arm_undef(cpu, bb, instr);
         int single = (BIT(8) == 0);
@@ -1482,44 +1211,26 @@ int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc
         return No_exp;
 }
 #endif
-#undef vfpinstr
-#undef vfpinstr_inst
-#undef VFPLABEL_INST
 
 /* ----------------------------------------------------------------------- */
 /* VMOVR move register */
 /* cond 1110 1D11 0000 Vd-- 101X 01M0 Vm-- */
 /* cond 1110 opc1 CRn- CRd- copr op20 CRm- CDP */
-#define vfpinstr        vmovr
-#define vfpinstr_inst   vmovr_inst
-#define VFPLABEL_INST   VMOVR_INST
-#ifdef VFP_DECODE
-{"vmov(r)",       5,      ARMVFP3,        23, 27, 0x1d,   16, 21, 0x30,    9, 11, 0x5,    6, 7, 1,        4, 4, 0},
-#endif
-#ifdef VFP_DECODE_EXCLUSION
-{"vmov(r)",       0,      ARMVFP3, 0},
-#endif
-#ifdef VFP_INTERPRETER_TABLE
-INTERPRETER_TRANSLATE(vfpinstr),
-#endif
-#ifdef VFP_INTERPRETER_LABEL
-&&VFPLABEL_INST,
-#endif
 #ifdef VFP_INTERPRETER_STRUCT
 typedef struct _vmovr_inst {
         unsigned int single;
         unsigned int d;
         unsigned int m;
-} vfpinstr_inst;
+} vmovr_inst;
 #endif
 #ifdef VFP_INTERPRETER_TRANS
-ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
+ARM_INST_PTR INTERPRETER_TRANSLATE(vmovr)(unsigned int inst, int index)
 {
         VFP_DEBUG_TRANSLATE;
         VFP_DEBUG_UNTESTED(VMOVR);
         
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vfpinstr_inst));
-        vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vmovr_inst));
+        vmovr_inst *inst_cream = (vmovr_inst *)inst_base->component;
 
         inst_base->cond  = BITS(inst, 28, 31);
         inst_base->idx   = index;
@@ -1533,56 +1244,27 @@ ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
 }
 #endif
 #ifdef VFP_INTERPRETER_IMPL
-VFPLABEL_INST:
+VMOVR_INST:
 {
-        INC_ICOUNTER;
         if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
                 CHECK_VFP_ENABLED;
                 
-                vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+                vmovr_inst *inst_cream = (vmovr_inst *)inst_base->component;
 
                 VMOVR(cpu, inst_cream->single, inst_cream->d, inst_cream->m);
         }
         cpu->Reg[15] += GET_INST_SIZE(cpu);
-        INC_PC(sizeof(vfpinstr_inst));
+        INC_PC(sizeof(vmovr_inst));
         FETCH_INST;
         GOTO_NEXT_INST;
 }
 #endif
-#ifdef VFP_CDP_TRANS
-if ( (OPC_1 & 0xb) == 0xb && CRn == 0 && (OPC_2 & 0x6) == 0x2 )
-{
-        unsigned int single   = BIT(8) == 0;
-        unsigned int d        = (single ? BITS(12,15)<<1 | BIT(22) : BITS(12,15) | BIT(22)<<4);
-        unsigned int m        = (single ? BITS( 0, 3)<<1 | BIT( 5) : BITS( 0, 3) | BIT( 5)<<4);;
-        VMOVR(state, single, d, m);
-        return ARMul_DONE;
-}
-#endif
-#ifdef VFP_CDP_IMPL
-void VMOVR(ARMul_State * state, ARMword single, ARMword d, ARMword m)
-{
-        DBG("VMOV(R) :\n");
-                
-        if (single)
-        {
-                DBG("\ts%d <= s%d[%x]\n", d, m, state->ExtReg[m]);
-                state->ExtReg[d] = state->ExtReg[m];
-        }
-        else
-        {
-                /* Check endian please */
-                DBG("\ts[%d-%d] <= s[%d-%d][%x-%x]\n", d*2+1, d*2, m*2+1, m*2, state->ExtReg[m*2+1], state->ExtReg[m*2]);
-                state->ExtReg[d*2+1] = state->ExtReg[m*2+1];
-                state->ExtReg[d*2] = state->ExtReg[m*2];
-        }
-}
-#endif
+
 #ifdef VFP_DYNCOM_TABLE
-DYNCOM_FILL_ACTION(vfpinstr),
+DYNCOM_FILL_ACTION(vmovr),
 #endif
 #ifdef VFP_DYNCOM_TAG
-int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
+int DYNCOM_TAG(vmovr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
 {
         int instr_size = INSTR_SIZE;
         arm_tag_continue(cpu, pc, instr, tag, new_pc, next_pc);
@@ -1594,7 +1276,7 @@ int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr
 }
 #endif
 #ifdef VFP_DYNCOM_TRANS
-int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
+int DYNCOM_TRANS(vmovr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
         DBG("\t\tin %s VMOV \n", __FUNCTION__);
         int single   = BIT(8) == 0;
         int d        = (single ? BITS(12,15)<<1 | BIT(22) : BIT(22) << 4 | BITS(12,15));
@@ -1613,41 +1295,23 @@ int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc
         return No_exp;
 }
 #endif
-#undef vfpinstr
-#undef vfpinstr_inst
-#undef VFPLABEL_INST
 
 /* ----------------------------------------------------------------------- */
 /* VABS */
 /* cond 1110 1D11 0000 Vd-- 101X 11M0 Vm-- */
-#define vfpinstr        vabs
-#define vfpinstr_inst   vabs_inst
-#define VFPLABEL_INST   VABS_INST
-#ifdef VFP_DECODE
-{"vabs",        5,      ARMVFP2,        23, 27, 0x1d,  16, 21, 0x30,    9, 11, 0x5,      6, 7, 3,     4, 4, 0},
-#endif
-#ifdef VFP_DECODE_EXCLUSION
-{"vabs",        0,      ARMVFP2, 0},
-#endif
-#ifdef VFP_INTERPRETER_TABLE
-INTERPRETER_TRANSLATE(vfpinstr),
-#endif
-#ifdef VFP_INTERPRETER_LABEL
-&&VFPLABEL_INST,
-#endif
 #ifdef VFP_INTERPRETER_STRUCT
 typedef struct _vabs_inst {
         unsigned int instr;
         unsigned int dp_operation;
-} vfpinstr_inst;
+} vabs_inst;
 #endif
 #ifdef VFP_INTERPRETER_TRANS
-ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
+ARM_INST_PTR INTERPRETER_TRANSLATE(vabs)(unsigned int inst, int index)
 {
         VFP_DEBUG_TRANSLATE;VFP_DEBUG_UNTESTED(VABS);
         
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vfpinstr_inst));
-        vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vabs_inst));
+        vabs_inst *inst_cream = (vabs_inst *)inst_base->component;
 
         inst_base->cond  = BITS(inst, 28, 31);
         inst_base->idx   = index;
@@ -1661,15 +1325,14 @@ ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
 }
 #endif
 #ifdef VFP_INTERPRETER_IMPL
-VFPLABEL_INST:
+VABS_INST:
 {
-        INC_ICOUNTER;
         if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
                 CHECK_VFP_ENABLED;
                 
                 DBG("VABS :\n");
 
-                vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+                vabs_inst *inst_cream = (vabs_inst *)inst_base->component;
 
                 int ret;
                 
@@ -1681,22 +1344,17 @@ VFPLABEL_INST:
                 CHECK_VFP_CDP_RET;
         }
         cpu->Reg[15] += GET_INST_SIZE(cpu);
-        INC_PC(sizeof(vfpinstr_inst));
+        INC_PC(sizeof(vabs_inst));
         FETCH_INST;
         GOTO_NEXT_INST;
 }
 #endif
-#ifdef VFP_CDP_TRANS
-if ((OPC_1 & 0xB) == 0xB && CRn == 0 && (OPC_2 & 0x7) == 6)
-{
-        DBG("VABS :\n");
-}
-#endif
+
 #ifdef VFP_DYNCOM_TABLE
-DYNCOM_FILL_ACTION(vfpinstr),
+DYNCOM_FILL_ACTION(vabs),
 #endif
 #ifdef VFP_DYNCOM_TAG
-int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
+int DYNCOM_TAG(vabs)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
 {
         int instr_size = INSTR_SIZE;
         //DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
@@ -1706,7 +1364,7 @@ int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr
 }
 #endif
 #ifdef VFP_DYNCOM_TRANS
-int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
+int DYNCOM_TRANS(vabs)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
         //DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
         //arch_arm_undef(cpu, bb, instr);
         int single   = BIT(8) == 0;
@@ -1744,42 +1402,24 @@ int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc
         return No_exp;
 }
 #endif
-#undef vfpinstr
-#undef vfpinstr_inst
-#undef VFPLABEL_INST
 
 /* ----------------------------------------------------------------------- */
 /* VNEG */
 /* cond 1110 1D11 0001 Vd-- 101X 11M0 Vm-- */
-#define vfpinstr        vneg
-#define vfpinstr_inst   vneg_inst
-#define VFPLABEL_INST   VNEG_INST
-#ifdef VFP_DECODE
-//{"vneg",        5,      ARMVFP2,        23, 27, 0x1d,  16, 21, 0x30,    9, 11, 0x5,      6, 7, 1,     4, 4, 0},
-{"vneg",        5,      ARMVFP2,        23, 27, 0x1d,  17, 21, 0x18,    9, 11, 0x5,      6, 7, 1,     4, 4, 0},
-#endif
-#ifdef VFP_DECODE_EXCLUSION
-{"vneg",        0,      ARMVFP2, 0},
-#endif
-#ifdef VFP_INTERPRETER_TABLE
-INTERPRETER_TRANSLATE(vfpinstr),
-#endif
-#ifdef VFP_INTERPRETER_LABEL
-&&VFPLABEL_INST,
-#endif
+
 #ifdef VFP_INTERPRETER_STRUCT
 typedef struct _vneg_inst {
         unsigned int instr;
         unsigned int dp_operation;
-} vfpinstr_inst;
+} vneg_inst;
 #endif
 #ifdef VFP_INTERPRETER_TRANS
-ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
+ARM_INST_PTR INTERPRETER_TRANSLATE(vneg)(unsigned int inst, int index)
 {
         VFP_DEBUG_TRANSLATE;VFP_DEBUG_UNTESTED(VNEG);
         
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vfpinstr_inst));
-        vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vneg_inst));
+        vneg_inst *inst_cream = (vneg_inst *)inst_base->component;
 
         inst_base->cond  = BITS(inst, 28, 31);
         inst_base->idx   = index;
@@ -1793,15 +1433,14 @@ ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
 }
 #endif
 #ifdef VFP_INTERPRETER_IMPL
-VFPLABEL_INST:
+VNEG_INST:
 {
-        INC_ICOUNTER;
         if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
                 CHECK_VFP_ENABLED;
 
                 DBG("VNEG :\n");
 
-                vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+                vneg_inst *inst_cream = (vneg_inst *)inst_base->component;
 
                 int ret;
                 
@@ -1813,22 +1452,17 @@ VFPLABEL_INST:
                 CHECK_VFP_CDP_RET;
         }
         cpu->Reg[15] += GET_INST_SIZE(cpu);
-        INC_PC(sizeof(vfpinstr_inst));
+        INC_PC(sizeof(vneg_inst));
         FETCH_INST;
         GOTO_NEXT_INST;
 }
 #endif
-#ifdef VFP_CDP_TRANS
-if ((OPC_1 & 0xB) == 0xB && CRn == 1 && (OPC_2 & 0x7) == 2)
-{
-        DBG("VNEG :\n");
-}
-#endif
+
 #ifdef VFP_DYNCOM_TABLE
-DYNCOM_FILL_ACTION(vfpinstr),
+DYNCOM_FILL_ACTION(vneg),
 #endif
 #ifdef VFP_DYNCOM_TAG
-int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
+int DYNCOM_TAG(vneg)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
 {
         int instr_size = INSTR_SIZE;
         DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
@@ -1838,7 +1472,7 @@ int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr
 }
 #endif
 #ifdef VFP_DYNCOM_TRANS
-int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
+int DYNCOM_TRANS(vneg)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
         DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
         //arch_arm_undef(cpu, bb, instr);
         int single   = BIT(8) == 0;
@@ -1876,41 +1510,23 @@ int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc
         return No_exp;
 }
 #endif
-#undef vfpinstr
-#undef vfpinstr_inst
-#undef VFPLABEL_INST
 
 /* ----------------------------------------------------------------------- */
 /* VSQRT */
 /* cond 1110 1D11 0001 Vd-- 101X 11M0 Vm-- */
-#define vfpinstr        vsqrt
-#define vfpinstr_inst   vsqrt_inst
-#define VFPLABEL_INST   VSQRT_INST
-#ifdef VFP_DECODE
-{"vsqrt",        5,      ARMVFP2,        23, 27, 0x1d,  16, 21, 0x31,    9, 11, 0x5,      6, 7, 3,     4, 4, 0},
-#endif
-#ifdef VFP_DECODE_EXCLUSION
-{"vsqrt",        0,      ARMVFP2, 0},
-#endif
-#ifdef VFP_INTERPRETER_TABLE
-INTERPRETER_TRANSLATE(vfpinstr),
-#endif
-#ifdef VFP_INTERPRETER_LABEL
-&&VFPLABEL_INST,
-#endif
 #ifdef VFP_INTERPRETER_STRUCT
 typedef struct _vsqrt_inst {
         unsigned int instr;
         unsigned int dp_operation;
-} vfpinstr_inst;
+} vsqrt_inst;
 #endif
 #ifdef VFP_INTERPRETER_TRANS
-ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
+ARM_INST_PTR INTERPRETER_TRANSLATE(vsqrt)(unsigned int inst, int index)
 {
         VFP_DEBUG_TRANSLATE;
         
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vfpinstr_inst));
-        vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vsqrt_inst));
+        vsqrt_inst *inst_cream = (vsqrt_inst *)inst_base->component;
 
         inst_base->cond  = BITS(inst, 28, 31);
         inst_base->idx   = index;
@@ -1924,15 +1540,14 @@ ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
 }
 #endif
 #ifdef VFP_INTERPRETER_IMPL
-VFPLABEL_INST:
+VSQRT_INST:
 {
-        INC_ICOUNTER;
         if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
                 CHECK_VFP_ENABLED;
                 
                 DBG("VSQRT :\n");
                 
-                vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+                vsqrt_inst *inst_cream = (vsqrt_inst *)inst_base->component;
 
                 int ret;
                 
@@ -1944,22 +1559,17 @@ VFPLABEL_INST:
                 CHECK_VFP_CDP_RET;
         }
         cpu->Reg[15] += GET_INST_SIZE(cpu);
-        INC_PC(sizeof(vfpinstr_inst));
+        INC_PC(sizeof(vsqrt_inst));
         FETCH_INST;
         GOTO_NEXT_INST;
 }
 #endif
-#ifdef VFP_CDP_TRANS
-if ((OPC_1 & 0xB) == 0xB && CRn == 1 && (OPC_2 & 0x7) == 6)
-{
-        DBG("VSQRT :\n");
-}
-#endif
+
 #ifdef VFP_DYNCOM_TABLE
-DYNCOM_FILL_ACTION(vfpinstr),
+DYNCOM_FILL_ACTION(vsqrt),
 #endif
 #ifdef VFP_DYNCOM_TAG
-int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
+int DYNCOM_TAG(vsqrt)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
 {
         int instr_size = INSTR_SIZE;
         DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
@@ -1969,7 +1579,7 @@ int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr
 }
 #endif
 #ifdef VFP_DYNCOM_TRANS
-int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
+int DYNCOM_TRANS(vsqrt)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
         DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
         //arch_arm_undef(cpu, bb, instr);
         int dp_op = (BIT(8) == 1);
@@ -1995,41 +1605,23 @@ int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc
         return No_exp;
 }
 #endif
-#undef vfpinstr
-#undef vfpinstr_inst
-#undef VFPLABEL_INST
 
 /* ----------------------------------------------------------------------- */
 /* VCMP VCMPE */
 /* cond 1110 1D11 0100 Vd-- 101X E1M0 Vm-- Encoding 1 */
-#define vfpinstr        vcmp
-#define vfpinstr_inst   vcmp_inst
-#define VFPLABEL_INST   VCMP_INST
-#ifdef VFP_DECODE
-{"vcmp",        5,      ARMVFP2,        23, 27, 0x1d,  16, 21, 0x34,    9, 11, 0x5,      6, 6, 1,     4, 4, 0},
-#endif
-#ifdef VFP_DECODE_EXCLUSION
-{"vcmp",        0,      ARMVFP2, 0},
-#endif
-#ifdef VFP_INTERPRETER_TABLE
-INTERPRETER_TRANSLATE(vfpinstr),
-#endif
-#ifdef VFP_INTERPRETER_LABEL
-&&VFPLABEL_INST,
-#endif
 #ifdef VFP_INTERPRETER_STRUCT
 typedef struct _vcmp_inst {
         unsigned int instr;
         unsigned int dp_operation;
-} vfpinstr_inst;
+} vcmp_inst;
 #endif
 #ifdef VFP_INTERPRETER_TRANS
-ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
+ARM_INST_PTR INTERPRETER_TRANSLATE(vcmp)(unsigned int inst, int index)
 {
         VFP_DEBUG_TRANSLATE;
         
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vfpinstr_inst));
-        vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vcmp_inst));
+        vcmp_inst *inst_cream = (vcmp_inst *)inst_base->component;
 
         inst_base->cond  = BITS(inst, 28, 31);
         inst_base->idx   = index;
@@ -2043,15 +1635,14 @@ ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
 }
 #endif
 #ifdef VFP_INTERPRETER_IMPL
-VFPLABEL_INST:
+VCMP_INST:
 {
-        INC_ICOUNTER;
         if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
                 CHECK_VFP_ENABLED;
 
                 DBG("VCMP(1) :\n");
 
-                vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+                vcmp_inst *inst_cream = (vcmp_inst *)inst_base->component;
 
                 int ret;
                 
@@ -2063,22 +1654,17 @@ VFPLABEL_INST:
                 CHECK_VFP_CDP_RET;
         }
         cpu->Reg[15] += GET_INST_SIZE(cpu);
-        INC_PC(sizeof(vfpinstr_inst));
+        INC_PC(sizeof(vcmp_inst));
         FETCH_INST;
         GOTO_NEXT_INST;
 }
 #endif
-#ifdef VFP_CDP_TRANS
-if ((OPC_1 & 0xB) == 0xB && CRn == 4 && (OPC_2 & 0x2) == 2)
-{
-        DBG("VCMP(1) :\n");
-}
-#endif
+
 #ifdef VFP_DYNCOM_TABLE
-DYNCOM_FILL_ACTION(vfpinstr),
+DYNCOM_FILL_ACTION(vcmp),
 #endif
 #ifdef VFP_DYNCOM_TAG
-int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
+int DYNCOM_TAG(vcmp)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
 {
         int instr_size = INSTR_SIZE;
         //arm_tag_trap(cpu, pc, instr, tag, new_pc, next_pc);
@@ -2087,7 +1673,7 @@ int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr
 }
 #endif
 #ifdef VFP_DYNCOM_TRANS
-int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
+int DYNCOM_TRANS(vcmp)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
         DBG("\t\tin %s instruction is executed out of JIT.\n", __FUNCTION__);
         //arch_arm_undef(cpu, bb, instr);
         int dp_op = (BIT(8) == 1);
@@ -2141,41 +1727,23 @@ int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc
         return No_exp;
 }
 #endif
-#undef vfpinstr
-#undef vfpinstr_inst
-#undef VFPLABEL_INST
 
 /* ----------------------------------------------------------------------- */
 /* VCMP VCMPE */
 /* cond 1110 1D11 0100 Vd-- 101X E1M0 Vm-- Encoding 2 */
-#define vfpinstr        vcmp2
-#define vfpinstr_inst   vcmp2_inst
-#define VFPLABEL_INST   VCMP2_INST
-#ifdef VFP_DECODE
-{"vcmp2",        5,      ARMVFP2,        23, 27, 0x1d,  16, 21, 0x35,    9, 11, 0x5,     0, 6, 0x40},
-#endif
-#ifdef VFP_DECODE_EXCLUSION
-{"vcmp2",        0,      ARMVFP2, 0},
-#endif
-#ifdef VFP_INTERPRETER_TABLE
-INTERPRETER_TRANSLATE(vfpinstr),
-#endif
-#ifdef VFP_INTERPRETER_LABEL
-&&VFPLABEL_INST,
-#endif
 #ifdef VFP_INTERPRETER_STRUCT
 typedef struct _vcmp2_inst {
         unsigned int instr;
         unsigned int dp_operation;
-} vfpinstr_inst;
+} vcmp2_inst;
 #endif
 #ifdef VFP_INTERPRETER_TRANS
-ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
+ARM_INST_PTR INTERPRETER_TRANSLATE(vcmp2)(unsigned int inst, int index)
 {
         VFP_DEBUG_TRANSLATE;
         
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vfpinstr_inst));
-        vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vcmp2_inst));
+        vcmp2_inst *inst_cream = (vcmp2_inst *)inst_base->component;
 
         inst_base->cond  = BITS(inst, 28, 31);
         inst_base->idx   = index;
@@ -2189,15 +1757,14 @@ ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
 }
 #endif
 #ifdef VFP_INTERPRETER_IMPL
-VFPLABEL_INST:
+VCMP2_INST:
 {
-        INC_ICOUNTER;
         if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
                 CHECK_VFP_ENABLED;
                 
                 DBG("VCMP(2) :\n");
 
-                vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+                vcmp2_inst *inst_cream = (vcmp2_inst *)inst_base->component;
 
                 int ret;
                 
@@ -2209,22 +1776,17 @@ VFPLABEL_INST:
                 CHECK_VFP_CDP_RET;
         }
         cpu->Reg[15] += GET_INST_SIZE(cpu);
-        INC_PC(sizeof(vfpinstr_inst));
+        INC_PC(sizeof(vcmp2_inst));
         FETCH_INST;
         GOTO_NEXT_INST;
 }
 #endif
-#ifdef VFP_CDP_TRANS
-if ((OPC_1 & 0xB) == 0xB && CRn == 5 && (OPC_2 & 0x2) == 2 && CRm == 0)
-{
-        DBG("VCMP(2) :\n");
-}
-#endif
+
 #ifdef VFP_DYNCOM_TABLE
-DYNCOM_FILL_ACTION(vfpinstr),
+DYNCOM_FILL_ACTION(vcmp2),
 #endif
 #ifdef VFP_DYNCOM_TAG
-int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
+int DYNCOM_TAG(vcmp2)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
 {
         int instr_size = INSTR_SIZE;
         //arm_tag_trap(cpu, pc, instr, tag, new_pc, next_pc);
@@ -2233,7 +1795,7 @@ int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr
 }
 #endif
 #ifdef VFP_DYNCOM_TRANS
-int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
+int DYNCOM_TRANS(vcmp2)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
         DBG("\t\tin %s instruction will executed out of JIT.\n", __FUNCTION__);
         //arch_arm_undef(cpu, bb, instr);
         int dp_op = (BIT(8) == 1);
@@ -2287,41 +1849,23 @@ int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc
         return No_exp;
 }
 #endif
-#undef vfpinstr
-#undef vfpinstr_inst
-#undef VFPLABEL_INST
 
 /* ----------------------------------------------------------------------- */
 /* VCVTBDS between double and single */
 /* cond 1110 1D11 0111 Vd-- 101X 11M0 Vm-- */
-#define vfpinstr        vcvtbds
-#define vfpinstr_inst   vcvtbds_inst
-#define VFPLABEL_INST   VCVTBDS_INST
-#ifdef VFP_DECODE
-{"vcvt(bds)",   5,      ARMVFP2,        23, 27, 0x1d,  16, 21, 0x37,    9, 11, 0x5,      6, 7, 3,     4, 4, 0},
-#endif
-#ifdef VFP_DECODE_EXCLUSION
-{"vcvt(bds)",        0,      ARMVFP2, 0},
-#endif
-#ifdef VFP_INTERPRETER_TABLE
-INTERPRETER_TRANSLATE(vfpinstr),
-#endif
-#ifdef VFP_INTERPRETER_LABEL
-&&VFPLABEL_INST,
-#endif
 #ifdef VFP_INTERPRETER_STRUCT
 typedef struct _vcvtbds_inst {
         unsigned int instr;
         unsigned int dp_operation;
-} vfpinstr_inst;
+} vcvtbds_inst;
 #endif
 #ifdef VFP_INTERPRETER_TRANS
-ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
+ARM_INST_PTR INTERPRETER_TRANSLATE(vcvtbds)(unsigned int inst, int index)
 {
         VFP_DEBUG_TRANSLATE;
         
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vfpinstr_inst));
-        vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vcvtbds_inst));
+        vcvtbds_inst *inst_cream = (vcvtbds_inst *)inst_base->component;
 
         inst_base->cond  = BITS(inst, 28, 31);
         inst_base->idx   = index;
@@ -2335,15 +1879,14 @@ ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
 }
 #endif
 #ifdef VFP_INTERPRETER_IMPL
-VFPLABEL_INST:
+VCVTBDS_INST:
 {
-        INC_ICOUNTER;
         if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
                 CHECK_VFP_ENABLED;
                 
                 DBG("VCVT(BDS) :\n");
 
-                vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+                vcvtbds_inst *inst_cream = (vcvtbds_inst *)inst_base->component;
 
                 int ret;
                 
@@ -2355,22 +1898,17 @@ VFPLABEL_INST:
                 CHECK_VFP_CDP_RET;
         }
         cpu->Reg[15] += GET_INST_SIZE(cpu);
-        INC_PC(sizeof(vfpinstr_inst));
+        INC_PC(sizeof(vcvtbds_inst));
         FETCH_INST;
         GOTO_NEXT_INST;
 }
 #endif
-#ifdef VFP_CDP_TRANS
-if ((OPC_1 & 0xB) == 0xB && CRn == 7 && (OPC_2 & 0x6) == 6)
-{
-        DBG("VCVT(BDS) :\n");
-}
-#endif
+
 #ifdef VFP_DYNCOM_TABLE
-DYNCOM_FILL_ACTION(vfpinstr),
+DYNCOM_FILL_ACTION(vcvtbds),
 #endif
 #ifdef VFP_DYNCOM_TAG
-int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
+int DYNCOM_TAG(vcvtbds)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
 {
         int instr_size = INSTR_SIZE;
         //arm_tag_trap(cpu, pc, instr, tag, new_pc, next_pc);
@@ -2379,7 +1917,7 @@ int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr
 }
 #endif
 #ifdef VFP_DYNCOM_TRANS
-int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
+int DYNCOM_TRANS(vcvtbds)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
         DBG("\t\tin %s instruction is executed out.\n", __FUNCTION__);
         //arch_arm_undef(cpu, bb, instr);
         int dp_op = (BIT(8) == 1);
@@ -2407,41 +1945,23 @@ int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc
         return No_exp;
 }
 #endif
-#undef vfpinstr
-#undef vfpinstr_inst
-#undef VFPLABEL_INST
 
 /* ----------------------------------------------------------------------- */
 /* VCVTBFF between floating point and fixed point */
 /* cond 1110 1D11 1op2 Vd-- 101X X1M0 Vm-- */
-#define vfpinstr        vcvtbff
-#define vfpinstr_inst   vcvtbff_inst
-#define VFPLABEL_INST   VCVTBFF_INST
-#ifdef VFP_DECODE
-{"vcvt(bff)",   6,      ARMVFP3,        23, 27, 0x1d,  19, 21, 0x7,     17, 17, 0x1,      9, 11, 0x5,   6, 6, 1},
-#endif
-#ifdef VFP_DECODE_EXCLUSION
-{"vcvt(bff)",   0,      ARMVFP3,         4, 4, 1},
-#endif
-#ifdef VFP_INTERPRETER_TABLE
-INTERPRETER_TRANSLATE(vfpinstr),
-#endif
-#ifdef VFP_INTERPRETER_LABEL
-&&VFPLABEL_INST,
-#endif
 #ifdef VFP_INTERPRETER_STRUCT
 typedef struct _vcvtbff_inst {
         unsigned int instr;
         unsigned int dp_operation;
-} vfpinstr_inst;
+} vcvtbff_inst;
 #endif
 #ifdef VFP_INTERPRETER_TRANS
-ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
+ARM_INST_PTR INTERPRETER_TRANSLATE(vcvtbff)(unsigned int inst, int index)
 {
         VFP_DEBUG_TRANSLATE;VFP_DEBUG_UNTESTED(VCVTBFF);
         
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vfpinstr_inst));
-        vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vcvtbff_inst));
+        vcvtbff_inst *inst_cream = (vcvtbff_inst *)inst_base->component;
 
         inst_base->cond  = BITS(inst, 28, 31);
         inst_base->idx   = index;
@@ -2455,15 +1975,14 @@ ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
 }
 #endif
 #ifdef VFP_INTERPRETER_IMPL
-VFPLABEL_INST:
+VCVTBFF_INST:
 {
-        INC_ICOUNTER;
         if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
                 CHECK_VFP_ENABLED;
                 
                 DBG("VCVT(BFF) :\n");
 
-                vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+                vcvtbff_inst *inst_cream = (vcvtbff_inst *)inst_base->component;
 
                 int ret;
                 
@@ -2475,22 +1994,17 @@ VFPLABEL_INST:
                 CHECK_VFP_CDP_RET;
         }
         cpu->Reg[15] += GET_INST_SIZE(cpu);
-        INC_PC(sizeof(vfpinstr_inst));
+        INC_PC(sizeof(vcvtbff_inst));
         FETCH_INST;
         GOTO_NEXT_INST;
 }
 #endif
-#ifdef VFP_CDP_TRANS
-if ((OPC_1 & 0xB) == 0xB && CRn >= 0xA && (OPC_2 & 0x2) == 2)
-{
-        DBG("VCVT(BFF) :\n");
-}
-#endif
+
 #ifdef VFP_DYNCOM_TABLE
-DYNCOM_FILL_ACTION(vfpinstr),
+DYNCOM_FILL_ACTION(vcvtbff),
 #endif
 #ifdef VFP_DYNCOM_TAG
-int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
+int DYNCOM_TAG(vcvtbff)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
 {
         int instr_size = INSTR_SIZE;
         DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
@@ -2499,47 +2013,29 @@ int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr
 }
 #endif
 #ifdef VFP_DYNCOM_TRANS
-int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
+int DYNCOM_TRANS(vcvtbff)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
         DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
         arch_arm_undef(cpu, bb, instr);
         return No_exp;
 }
 #endif
-#undef vfpinstr
-#undef vfpinstr_inst
-#undef VFPLABEL_INST
 
 /* ----------------------------------------------------------------------- */
 /* VCVTBFI between floating point and integer */
 /* cond 1110 1D11 1op2 Vd-- 101X X1M0 Vm-- */
-#define vfpinstr        vcvtbfi
-#define vfpinstr_inst   vcvtbfi_inst
-#define VFPLABEL_INST   VCVTBFI_INST
-#ifdef VFP_DECODE
-{"vcvt(bfi)",   5,      ARMVFP2,        23, 27, 0x1d,  19, 21, 0x7,     9, 11, 0x5,      6, 6, 1,     4, 4, 0},
-#endif
-#ifdef VFP_DECODE_EXCLUSION
-{"vcvt(bfi)",   0,      ARMVFP2, 0},
-#endif
-#ifdef VFP_INTERPRETER_TABLE
-INTERPRETER_TRANSLATE(vfpinstr),
-#endif
-#ifdef VFP_INTERPRETER_LABEL
-&&VFPLABEL_INST,
-#endif
 #ifdef VFP_INTERPRETER_STRUCT
 typedef struct _vcvtbfi_inst {
         unsigned int instr;
         unsigned int dp_operation;
-} vfpinstr_inst;
+} vcvtbfi_inst;
 #endif
 #ifdef VFP_INTERPRETER_TRANS
-ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
+ARM_INST_PTR INTERPRETER_TRANSLATE(vcvtbfi)(unsigned int inst, int index)
 {
         VFP_DEBUG_TRANSLATE;
         
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vfpinstr_inst));
-        vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vcvtbfi_inst));
+        vcvtbfi_inst *inst_cream = (vcvtbfi_inst *)inst_base->component;
 
         inst_base->cond  = BITS(inst, 28, 31);
         inst_base->idx   = index;
@@ -2554,15 +2050,14 @@ ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
 }
 #endif
 #ifdef VFP_INTERPRETER_IMPL
-VFPLABEL_INST:
+VCVTBFI_INST:
 {
-        INC_ICOUNTER;
         if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
                 CHECK_VFP_ENABLED;
                 
                 DBG("VCVT(BFI) :\n");
                 
-                vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+                vcvtbfi_inst *inst_cream = (vcvtbfi_inst *)inst_base->component;
 
                 int ret;
                 
@@ -2574,22 +2069,17 @@ VFPLABEL_INST:
                 CHECK_VFP_CDP_RET;
         }
         cpu->Reg[15] += GET_INST_SIZE(cpu);
-        INC_PC(sizeof(vfpinstr_inst));
+        INC_PC(sizeof(vcvtbfi_inst));
         FETCH_INST;
         GOTO_NEXT_INST;
 }
 #endif
-#ifdef VFP_CDP_TRANS
-if ((OPC_1 & 0xB) == 0xB && CRn > 7 && (OPC_2 & 0x2) == 2)
-{
-        DBG("VCVT(BFI) :\n");
-}
-#endif
+
 #ifdef VFP_DYNCOM_TABLE
-DYNCOM_FILL_ACTION(vfpinstr),
+DYNCOM_FILL_ACTION(vcvtbfi),
 #endif
 #ifdef VFP_DYNCOM_TAG
-int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
+int DYNCOM_TAG(vcvtbfi)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
 {
         int instr_size = INSTR_SIZE;
         //DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
@@ -2600,7 +2090,7 @@ int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr
 }
 #endif
 #ifdef VFP_DYNCOM_TRANS
-int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
+int DYNCOM_TRANS(vcvtbfi)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
         DBG("\t\tin %s, instruction will be executed out of JIT.\n", __FUNCTION__);
         //arch_arm_undef(cpu, bb, instr);
         unsigned int opc2 = BITS(16,18);
@@ -2694,9 +2184,6 @@ int vcvtbfi_instr_impl(arm_core_t* cpu, uint32 instr){
         return 0;
 }
 #endif
-#undef vfpinstr
-#undef vfpinstr_inst
-#undef VFPLABEL_INST
 
 /* ----------------------------------------------------------------------- */
 /* MRC / MCR instructions */
@@ -2707,35 +2194,20 @@ int vcvtbfi_instr_impl(arm_core_t* cpu, uint32 instr){
 /* VMOVBRS between register and single precision */
 /* cond 1110 000o Vn-- Rt-- 1010 N001 0000 */
 /* cond 1110 op11 CRn- Rt-- copr op21 CRm- MRC */
-#define vfpinstr        vmovbrs
-#define vfpinstr_inst   vmovbrs_inst
-#define VFPLABEL_INST   VMOVBRS_INST
-#ifdef VFP_DECODE
-{"vmovbrs",    3,    ARMVFP2,    21, 27, 0x70,    8, 11, 0xA,    0, 6, 0x10},
-#endif
-#ifdef VFP_DECODE_EXCLUSION
-{"vmovbrs",    0,    ARMVFP2,    0},
-#endif
-#ifdef VFP_INTERPRETER_TABLE
-INTERPRETER_TRANSLATE(vfpinstr),
-#endif
-#ifdef VFP_INTERPRETER_LABEL
-&&VFPLABEL_INST,
-#endif
 #ifdef VFP_INTERPRETER_STRUCT
 typedef struct _vmovbrs_inst {
         unsigned int to_arm;
         unsigned int t;
         unsigned int n;
-} vfpinstr_inst;
+} vmovbrs_inst;
 #endif
 #ifdef VFP_INTERPRETER_TRANS
-ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
+ARM_INST_PTR INTERPRETER_TRANSLATE(vmovbrs)(unsigned int inst, int index)
 {
         VFP_DEBUG_TRANSLATE;
         
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vfpinstr_inst));
-        vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vmovbrs_inst));
+        vmovbrs_inst *inst_cream = (vmovbrs_inst *)inst_base->component;
 
         inst_base->cond  = BITS(inst, 28, 31);
         inst_base->idx     = index;
@@ -2750,61 +2222,27 @@ ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
 }
 #endif
 #ifdef VFP_INTERPRETER_IMPL
-VFPLABEL_INST:
+VMOVBRS_INST:
 {
-        INC_ICOUNTER;
         if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
                 CHECK_VFP_ENABLED;
-           
-                vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+
+                vmovbrs_inst *inst_cream = (vmovbrs_inst *)inst_base->component;
 
                 VMOVBRS(cpu, inst_cream->to_arm, inst_cream->t, inst_cream->n, &(cpu->Reg[inst_cream->t]));
         }
         cpu->Reg[15] += GET_INST_SIZE(cpu);
-        INC_PC(sizeof(vfpinstr_inst));
+        INC_PC(sizeof(vmovbrs_inst));
         FETCH_INST;
         GOTO_NEXT_INST;
 }
 #endif
-#ifdef VFP_MRC_TRANS
-if (OPC_1 == 0x0 && CRm == 0 && (OPC_2 & 0x3) == 0)
-{
-        /* VMOV r to s */
-        /* Transfering Rt is not mandatory, as the value of interest is pointed by value */
-        VMOVBRS(state, BIT(20), Rt, BIT(7)|CRn<<1, value);
-        return ARMul_DONE;
-}
-#endif
-#ifdef VFP_MCR_TRANS
-if (OPC_1 == 0x0 && CRm == 0 && (OPC_2 & 0x3) == 0)
-{
-        /* VMOV s to r */
-        /* Transfering Rt is not mandatory, as the value of interest is pointed by value */
-        VMOVBRS(state, BIT(20), Rt, BIT(7)|CRn<<1, &value);
-        return ARMul_DONE;
-}
-#endif
-#ifdef VFP_MRC_IMPL
-void VMOVBRS(ARMul_State * state, ARMword to_arm, ARMword t, ARMword n, ARMword *value)
-{
-        DBG("VMOV(BRS) :\n");
-        if (to_arm)
-        {
-                DBG("\tr%d <= s%d=[%x]\n", t, n, state->ExtReg[n]);
-                *value = state->ExtReg[n];
-        }
-        else
-        {
-                DBG("\ts%d <= r%d=[%x]\n", n, t, *value);
-                state->ExtReg[n] = *value;
-        }
-}
-#endif
+
 #ifdef VFP_DYNCOM_TABLE
-DYNCOM_FILL_ACTION(vfpinstr),
+DYNCOM_FILL_ACTION(vmovbrs),
 #endif
 #ifdef VFP_DYNCOM_TAG
-int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
+int DYNCOM_TAG(vmovbrs)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
 {
         int instr_size = INSTR_SIZE;
         //DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
@@ -2813,7 +2251,7 @@ int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr
 }
 #endif
 #ifdef VFP_DYNCOM_TRANS
-int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
+int DYNCOM_TRANS(vmovbrs)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
         DBG("VMOV(BRS) :\n");
         int to_arm   = BIT(20) == 1;
         int t        = BITS(12, 15);
@@ -2832,42 +2270,24 @@ int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc
         return No_exp;
 }
 #endif
-#undef vfpinstr
-#undef vfpinstr_inst
-#undef VFPLABEL_INST
 
 /* ----------------------------------------------------------------------- */
 /* VMSR */
 /* cond 1110 1110 reg- Rt-- 1010 0001 0000 */
 /* cond 1110 op10 CRn- Rt-- copr op21 CRm- MCR */
-#define vfpinstr        vmsr
-#define vfpinstr_inst   vmsr_inst
-#define VFPLABEL_INST   VMSR_INST
-#ifdef VFP_DECODE
-{"vmsr",    2,    ARMVFP2,    20, 27, 0xEE,    0, 11, 0xA10},
-#endif
-#ifdef VFP_DECODE_EXCLUSION
-{"vmsr",    0,    ARMVFP2,    0},
-#endif
-#ifdef VFP_INTERPRETER_TABLE
-INTERPRETER_TRANSLATE(vfpinstr),
-#endif
-#ifdef VFP_INTERPRETER_LABEL
-&&VFPLABEL_INST,
-#endif
 #ifdef VFP_INTERPRETER_STRUCT
 typedef struct _vmsr_inst {
         unsigned int reg;
         unsigned int Rd;
-} vfpinstr_inst;
+} vmsr_inst;
 #endif
 #ifdef VFP_INTERPRETER_TRANS
-ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
+ARM_INST_PTR INTERPRETER_TRANSLATE(vmsr)(unsigned int inst, int index)
 {
         VFP_DEBUG_TRANSLATE;
         
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vfpinstr_inst));
-        vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vmsr_inst));
+        vmsr_inst *inst_cream = (vmsr_inst *)inst_base->component;
 
         inst_base->cond  = BITS(inst, 28, 31);
         inst_base->idx     = index;
@@ -2881,52 +2301,30 @@ ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
 }
 #endif
 #ifdef VFP_INTERPRETER_IMPL
-VFPLABEL_INST:
+VMSR_INST:
 {
-        INC_ICOUNTER;
         if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
                 /* FIXME: special case for access to FPSID and FPEXC, VFP must be disabled ,
                    and in privilegied mode */
                 /* Exceptions must be checked, according to v7 ref manual */
                 CHECK_VFP_ENABLED;
            
-                vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+                vmsr_inst *inst_cream = (vmsr_inst *)inst_base->component;
 
                 VMSR(cpu, inst_cream->reg, inst_cream->Rd);
         }
         cpu->Reg[15] += GET_INST_SIZE(cpu);
-        INC_PC(sizeof(vfpinstr_inst));
+        INC_PC(sizeof(vmsr_inst));
         FETCH_INST;
         GOTO_NEXT_INST;
 }
 #endif
-#ifdef VFP_MCR_TRANS
-if (OPC_1 == 0x7 && CRm == 0 && OPC_2 == 0)
-{
-        VMSR(state, CRn, Rt);
-        return ARMul_DONE;
-}
-#endif
-#ifdef VFP_MCR_IMPL
-void VMSR(ARMul_State * state, ARMword reg, ARMword Rt)
-{
-        if (reg == 1)
-        {
-                DBG("VMSR :\tfpscr <= r%d=[%x]\n", Rt, state->Reg[Rt]);
-                state->VFP[VFP_OFFSET(VFP_FPSCR)] = state->Reg[Rt];
-        }
-        else if (reg == 8)
-        {
-                DBG("VMSR :\tfpexc <= r%d=[%x]\n", Rt, state->Reg[Rt]);
-                state->VFP[VFP_OFFSET(VFP_FPEXC)] = state->Reg[Rt];
-        }
-}
-#endif
+
 #ifdef VFP_DYNCOM_TABLE
-DYNCOM_FILL_ACTION(vfpinstr),
+DYNCOM_FILL_ACTION(vmsr),
 #endif
 #ifdef VFP_DYNCOM_TAG
-int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
+int DYNCOM_TAG(vmsr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
 {
         int instr_size = INSTR_SIZE;
         //DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
@@ -2936,7 +2334,7 @@ int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr
 }
 #endif
 #ifdef VFP_DYNCOM_TRANS
-int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
+int DYNCOM_TRANS(vmsr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
         //DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
         //arch_arm_undef(cpu, bb, instr);
         DBG("VMSR :");
@@ -2969,44 +2367,26 @@ int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc
         return No_exp;
 }
 #endif
-#undef vfpinstr
-#undef vfpinstr_inst
-#undef VFPLABEL_INST
 
 /* ----------------------------------------------------------------------- */
 /* VMOVBRC register to scalar */
 /* cond 1110 0XX0 Vd-- Rt-- 1011 DXX1 0000 */
 /* cond 1110 op10 CRn- Rt-- copr op21 CRm- MCR */
-#define vfpinstr        vmovbrc
-#define vfpinstr_inst   vmovbrc_inst
-#define VFPLABEL_INST   VMOVBRC_INST
-#ifdef VFP_DECODE
-{"vmovbrc",    4,    ARMVFP2,    23, 27, 0x1C,    20, 20, 0x0,    8,11,0xB,    0,4,0x10},
-#endif
-#ifdef VFP_DECODE_EXCLUSION
-{"vmovbrc",    0,    ARMVFP2,    0},
-#endif
-#ifdef VFP_INTERPRETER_TABLE
-INTERPRETER_TRANSLATE(vfpinstr),
-#endif
-#ifdef VFP_INTERPRETER_LABEL
-&&VFPLABEL_INST,
-#endif
 #ifdef VFP_INTERPRETER_STRUCT
 typedef struct _vmovbrc_inst {
         unsigned int esize;
         unsigned int index;
         unsigned int d;
         unsigned int t;
-} vfpinstr_inst;
+} vmovbrc_inst;
 #endif
 #ifdef VFP_INTERPRETER_TRANS
-ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
+ARM_INST_PTR INTERPRETER_TRANSLATE(vmovbrc)(unsigned int inst, int index)
 {
         VFP_DEBUG_TRANSLATE;
         
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vfpinstr_inst));
-        vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vmovbrc_inst));
+        vmovbrc_inst *inst_cream = (vmovbrc_inst *)inst_base->component;
 
         inst_base->cond  = BITS(inst, 28, 31);
         inst_base->idx     = index;
@@ -3023,34 +2403,27 @@ ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
 }
 #endif
 #ifdef VFP_INTERPRETER_IMPL
-VFPLABEL_INST:
+VMOVBRC_INST:
 {
-        INC_ICOUNTER;
         if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
                 CHECK_VFP_ENABLED;
                 
-                vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+                vmovbrc_inst *inst_cream = (vmovbrc_inst *)inst_base->component;
                 
                 VFP_DEBUG_UNIMPLEMENTED(VMOVBRC);
         }
         cpu->Reg[15] += GET_INST_SIZE(cpu);
-        INC_PC(sizeof(vfpinstr_inst));
+        INC_PC(sizeof(vmovbrc_inst));
         FETCH_INST;
         GOTO_NEXT_INST;
 }
 #endif
-#ifdef VFP_MCR_TRANS
-if ((OPC_1 & 0x4) == 0 && CoProc == 11 && CRm == 0)
-{
-        VFP_DEBUG_UNIMPLEMENTED(VMOVBRC);
-        return ARMul_DONE;
-}
-#endif
+
 #ifdef VFP_DYNCOM_TABLE
-DYNCOM_FILL_ACTION(vfpinstr),
+DYNCOM_FILL_ACTION(vmovbrc),
 #endif
 #ifdef VFP_DYNCOM_TAG
-int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
+int DYNCOM_TAG(vmovbrc)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
 {
         int instr_size = INSTR_SIZE;
         DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
@@ -3059,48 +2432,30 @@ int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr
 }
 #endif
 #ifdef VFP_DYNCOM_TRANS
-int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
+int DYNCOM_TRANS(vmovbrc)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
         DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
         arch_arm_undef(cpu, bb, instr);
         return No_exp;
 }
 #endif
-#undef vfpinstr
-#undef vfpinstr_inst
-#undef VFPLABEL_INST
 
 /* ----------------------------------------------------------------------- */
 /* VMRS */
 /* cond 1110 1111 CRn- Rt-- 1010 0001 0000 */
 /* cond 1110 op11 CRn- Rt-- copr op21 CRm- MRC */
-#define vfpinstr        vmrs
-#define vfpinstr_inst   vmrs_inst
-#define VFPLABEL_INST   VMRS_INST
-#ifdef VFP_DECODE
-{"vmrs",        2,      ARMVFP2,        20, 27, 0xEF,     0, 11, 0xa10},
-#endif
-#ifdef VFP_DECODE_EXCLUSION
-{"vmrs",    0,    ARMVFP2,    0},
-#endif
-#ifdef VFP_INTERPRETER_TABLE
-INTERPRETER_TRANSLATE(vfpinstr),
-#endif
-#ifdef VFP_INTERPRETER_LABEL
-&&VFPLABEL_INST,
-#endif
 #ifdef VFP_INTERPRETER_STRUCT
 typedef struct _vmrs_inst {
         unsigned int reg;
         unsigned int Rt;
-} vfpinstr_inst;
+} vmrs_inst;
 #endif
 #ifdef VFP_INTERPRETER_TRANS
-ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
+ARM_INST_PTR INTERPRETER_TRANSLATE(vmrs)(unsigned int inst, int index)
 {
         VFP_DEBUG_TRANSLATE;
         
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vfpinstr_inst));
-        vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vmrs_inst));
+        vmrs_inst *inst_cream = (vmrs_inst *)inst_base->component;
 
         inst_base->cond  = BITS(inst, 28, 31);
         inst_base->idx     = index;
@@ -3109,21 +2464,20 @@ ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
 
         inst_cream->reg  = BITS(inst, 16, 19);
         inst_cream->Rt   = BITS(inst, 12, 15);
-   
+
         return inst_base;
 }
 #endif
 #ifdef VFP_INTERPRETER_IMPL
-VFPLABEL_INST:
+VMRS_INST:
 {
-        INC_ICOUNTER;
         if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
                 /* FIXME: special case for access to FPSID and FPEXC, VFP must be disabled,
                    and in privilegied mode */
                 /* Exceptions must be checked, according to v7 ref manual */
                 CHECK_VFP_ENABLED;
                 
-                vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+                vmrs_inst *inst_cream = (vmrs_inst *)inst_base->component;
                 
                 DBG("VMRS :");
         
@@ -3170,67 +2524,17 @@ VFPLABEL_INST:
                 }
         }
         cpu->Reg[15] += GET_INST_SIZE(cpu);
-        INC_PC(sizeof(vfpinstr_inst));
+        INC_PC(sizeof(vmrs_inst));
         FETCH_INST;
         GOTO_NEXT_INST;
 }
 #endif
-#ifdef VFP_MRC_TRANS
-if (OPC_1 == 0x7 && CRm == 0 && OPC_2 == 0)
-{
-        VMRS(state, CRn, Rt, value);
-        return ARMul_DONE;
-}
-#endif
-#ifdef VFP_MRC_IMPL
-void VMRS(ARMul_State * state, ARMword reg, ARMword Rt, ARMword * value)
-{
-        DBG("VMRS :");
-        if (reg == 1)
-        {
-                if (Rt != 15)
-                {
-                        *value = state->VFP[VFP_OFFSET(VFP_FPSCR)];
-                        DBG("\tr%d <= fpscr[%08x]\n", Rt, state->VFP[VFP_OFFSET(VFP_FPSCR)]);
-                }
-                else
-                {
-                        *value = state->VFP[VFP_OFFSET(VFP_FPSCR)] ;
-                        DBG("\tflags <= fpscr[%1xxxxxxxx]\n", state->VFP[VFP_OFFSET(VFP_FPSCR)]>>28);
-                }
-        }
-        else
-        {
-                switch (reg)
-                {
-                case 0:
-                        *value = state->VFP[VFP_OFFSET(VFP_FPSID)];
-                        DBG("\tr%d <= fpsid[%08x]\n", Rt, state->VFP[VFP_OFFSET(VFP_FPSID)]);
-                        break;
-                case 6:
-                        /* MVFR1, VFPv3 only ? */
-                        DBG("\tr%d <= MVFR1 unimplemented\n", Rt);
-                        break;
-                case 7:
-                        /* MVFR0, VFPv3 only? */
-                        DBG("\tr%d <= MVFR0 unimplemented\n", Rt);
-                        break;
-                case 8:
-                        *value = state->VFP[VFP_OFFSET(VFP_FPEXC)];
-                        DBG("\tr%d <= fpexc[%08x]\n", Rt, state->VFP[VFP_OFFSET(VFP_FPEXC)]);
-                        break;
-                default:
-                        DBG("\tSUBARCHITECTURE DEFINED\n");
-                        break;
-                }
-        }
-}
-#endif
+
 #ifdef VFP_DYNCOM_TABLE
-DYNCOM_FILL_ACTION(vfpinstr),
+DYNCOM_FILL_ACTION(vmrs),
 #endif
 #ifdef VFP_DYNCOM_TAG
-int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
+int DYNCOM_TAG(vmrs)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
 {
         int instr_size = INSTR_SIZE;
         //DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
@@ -3241,7 +2545,7 @@ int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr
 }
 #endif
 #ifdef VFP_DYNCOM_TRANS
-int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
+int DYNCOM_TRANS(vmrs)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
         //DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
         //arch_arm_undef(cpu, bb, instr);
         
@@ -3292,44 +2596,26 @@ int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc
         return No_exp;
 }
 #endif
-#undef vfpinstr
-#undef vfpinstr_inst
-#undef VFPLABEL_INST
 
 /* ----------------------------------------------------------------------- */
 /* VMOVBCR scalar to register */
 /* cond 1110 XXX1 Vd-- Rt-- 1011 NXX1 0000 */
 /* cond 1110 op11 CRn- Rt-- copr op21 CRm- MCR */
-#define vfpinstr        vmovbcr
-#define vfpinstr_inst   vmovbcr_inst
-#define VFPLABEL_INST   VMOVBCR_INST
-#ifdef VFP_DECODE
-{"vmovbcr",    4,    ARMVFP2,    24, 27, 0xE,    20, 20, 1,    8, 11,0xB,    0,4, 0x10},
-#endif
-#ifdef VFP_DECODE_EXCLUSION
-{"vmovbcr",    0,    ARMVFP2,    0},
-#endif
-#ifdef VFP_INTERPRETER_TABLE
-INTERPRETER_TRANSLATE(vfpinstr),
-#endif
-#ifdef VFP_INTERPRETER_LABEL
-&&VFPLABEL_INST,
-#endif
 #ifdef VFP_INTERPRETER_STRUCT
 typedef struct _vmovbcr_inst {
         unsigned int esize;
         unsigned int index;
         unsigned int d;
         unsigned int t;
-} vfpinstr_inst;
+} vmovbcr_inst;
 #endif
 #ifdef VFP_INTERPRETER_TRANS
-ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
+ARM_INST_PTR INTERPRETER_TRANSLATE(vmovbcr)(unsigned int inst, int index)
 {
         VFP_DEBUG_TRANSLATE;
         
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vfpinstr_inst));
-        vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vmovbcr_inst));
+        vmovbcr_inst *inst_cream = (vmovbcr_inst *)inst_base->component;
 
         inst_base->cond  = BITS(inst, 28, 31);
         inst_base->idx     = index;
@@ -3346,34 +2632,27 @@ ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
 }
 #endif
 #ifdef VFP_INTERPRETER_IMPL
-VFPLABEL_INST:
+VMOVBCR_INST:
 {
-        INC_ICOUNTER;
         if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
                 CHECK_VFP_ENABLED;
                 
-                vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+                vmovbcr_inst *inst_cream = (vmovbcr_inst *)inst_base->component;
                 
                 VFP_DEBUG_UNIMPLEMENTED(VMOVBCR);
         }
         cpu->Reg[15] += GET_INST_SIZE(cpu);
-        INC_PC(sizeof(vfpinstr_inst));
+        INC_PC(sizeof(vmovbcr_inst));
         FETCH_INST;
         GOTO_NEXT_INST;
 }
 #endif
-#ifdef VFP_MCR_TRANS
-if (CoProc == 11 && CRm == 0)
-{
-        VFP_DEBUG_UNIMPLEMENTED(VMOVBCR);
-        return ARMul_DONE;
-}
-#endif
+
 #ifdef VFP_DYNCOM_TABLE
-DYNCOM_FILL_ACTION(vfpinstr),
+DYNCOM_FILL_ACTION(vmovbcr),
 #endif
 #ifdef VFP_DYNCOM_TAG
-int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
+int DYNCOM_TAG(vmovbcr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
 {
         int instr_size = INSTR_SIZE;
         DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
@@ -3382,15 +2661,12 @@ int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr
 }
 #endif
 #ifdef VFP_DYNCOM_TRANS
-int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
+int DYNCOM_TRANS(vmovbcr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
         DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
         arch_arm_undef(cpu, bb, instr);
         return No_exp;
 }
 #endif
-#undef vfpinstr
-#undef vfpinstr_inst
-#undef VFPLABEL_INST
 
 /* ----------------------------------------------------------------------- */
 /* MRRC / MCRR instructions */
@@ -3401,36 +2677,21 @@ int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc
 /* VMOVBRRSS between 2 registers to 2 singles */
 /* cond 1100 010X Rt2- Rt-- 1010 00X1 Vm-- */
 /* cond 1100 0101 Rt2- Rt-- copr opc1 CRm- MRRC */
-#define vfpinstr        vmovbrrss
-#define vfpinstr_inst   vmovbrrss_inst
-#define VFPLABEL_INST   VMOVBRRSS_INST
-#ifdef VFP_DECODE
-{"vmovbrrss",    3,    ARMVFP2,    21, 27, 0x62,    8, 11, 0xA,    4, 4, 1},
-#endif
-#ifdef VFP_DECODE_EXCLUSION
-{"vmovbrrss",    0,    ARMVFP2,    0},
-#endif
-#ifdef VFP_INTERPRETER_TABLE
-INTERPRETER_TRANSLATE(vfpinstr),
-#endif
-#ifdef VFP_INTERPRETER_LABEL
-&&VFPLABEL_INST,
-#endif
 #ifdef VFP_INTERPRETER_STRUCT
 typedef struct _vmovbrrss_inst {
         unsigned int to_arm;
         unsigned int t;
         unsigned int t2;
         unsigned int m;
-} vfpinstr_inst;
+} vmovbrrss_inst;
 #endif
 #ifdef VFP_INTERPRETER_TRANS
-ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
+ARM_INST_PTR INTERPRETER_TRANSLATE(vmovbrrss)(unsigned int inst, int index)
 {
         VFP_DEBUG_TRANSLATE;
         
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vfpinstr_inst));
-        vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vmovbrrss_inst));
+        vmovbrrss_inst *inst_cream = (vmovbrrss_inst *)inst_base->component;
 
         inst_base->cond  = BITS(inst, 28, 31);
         inst_base->idx     = index;
@@ -3446,41 +2707,26 @@ ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
 }
 #endif
 #ifdef VFP_INTERPRETER_IMPL
-VFPLABEL_INST:
+VMOVBRRSS_INST:
 {
-        INC_ICOUNTER;
         if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
                 CHECK_VFP_ENABLED;
                 
-                vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+                vmovbrrss_inst *inst_cream = (vmovbrrss_inst *)inst_base->component;
                 
                 VFP_DEBUG_UNIMPLEMENTED(VMOVBRRSS);
         }
         cpu->Reg[15] += GET_INST_SIZE(cpu);
-        INC_PC(sizeof(vfpinstr_inst));
+        INC_PC(sizeof(vmovbrrss_inst));
         FETCH_INST;
         GOTO_NEXT_INST;
 }
 #endif
-#ifdef VFP_MCRR_TRANS
-if (CoProc == 10 && (OPC_1 & 0xD) == 1)
-{
-        VFP_DEBUG_UNIMPLEMENTED(VMOVBRRSS);
-        return ARMul_DONE;
-}
-#endif
-#ifdef VFP_MRRC_TRANS
-if (CoProc == 10 && (OPC_1 & 0xD) == 1)
-{
-        VFP_DEBUG_UNIMPLEMENTED(VMOVBRRSS);
-        return ARMul_DONE;
-}
-#endif
 #ifdef VFP_DYNCOM_TABLE
-DYNCOM_FILL_ACTION(vfpinstr),
+DYNCOM_FILL_ACTION(vmovbrrss),
 #endif
 #ifdef VFP_DYNCOM_TAG
-int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
+int DYNCOM_TAG(vmovbrrss)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
 {
         int instr_size = INSTR_SIZE;
         DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
@@ -3489,50 +2735,32 @@ int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr
 }
 #endif
 #ifdef VFP_DYNCOM_TRANS
-int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
+int DYNCOM_TRANS(vmovbrrss)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
         DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
         arch_arm_undef(cpu, bb, instr);
         return No_exp;
 }
 #endif
-#undef vfpinstr
-#undef vfpinstr_inst
-#undef VFPLABEL_INST
 
 /* ----------------------------------------------------------------------- */
 /* VMOVBRRD between 2 registers and 1 double */
 /* cond 1100 010X Rt2- Rt-- 1011 00X1 Vm-- */
 /* cond 1100 0101 Rt2- Rt-- copr opc1 CRm- MRRC */
-#define vfpinstr        vmovbrrd
-#define vfpinstr_inst   vmovbrrd_inst
-#define VFPLABEL_INST   VMOVBRRD_INST
-#ifdef VFP_DECODE
-{"vmovbrrd",    3,    ARMVFP2,    21, 27, 0x62,    6, 11, 0x2c,    4, 4, 1},
-#endif
-#ifdef VFP_DECODE_EXCLUSION
-{"vmovbrrd",    0,    ARMVFP2,    0},
-#endif
-#ifdef VFP_INTERPRETER_TABLE
-INTERPRETER_TRANSLATE(vfpinstr),
-#endif
-#ifdef VFP_INTERPRETER_LABEL
-&&VFPLABEL_INST,
-#endif
 #ifdef VFP_INTERPRETER_STRUCT
 typedef struct _vmovbrrd_inst {
         unsigned int to_arm;
         unsigned int t;
         unsigned int t2;
         unsigned int m;
-} vfpinstr_inst;
+} vmovbrrd_inst;
 #endif
 #ifdef VFP_INTERPRETER_TRANS
-ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
+ARM_INST_PTR INTERPRETER_TRANSLATE(vmovbrrd)(unsigned int inst, int index)
 {
         VFP_DEBUG_TRANSLATE;
         
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vfpinstr_inst));
-        vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vmovbrrd_inst));
+        vmovbrrd_inst *inst_cream = (vmovbrrd_inst *)inst_base->component;
 
         inst_base->cond  = BITS(inst, 28, 31);
         inst_base->idx     = index;
@@ -3548,63 +2776,28 @@ ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
 }
 #endif
 #ifdef VFP_INTERPRETER_IMPL
-VFPLABEL_INST:
+VMOVBRRD_INST:
 {
-        INC_ICOUNTER;
         if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
                 CHECK_VFP_ENABLED;
                 
-                vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+                vmovbrrd_inst *inst_cream = (vmovbrrd_inst *)inst_base->component;
                 
                 VMOVBRRD(cpu, inst_cream->to_arm, inst_cream->t, inst_cream->t2, inst_cream->m, 
                                 &(cpu->Reg[inst_cream->t]), &(cpu->Reg[inst_cream->t2]));
         }
         cpu->Reg[15] += GET_INST_SIZE(cpu);
-        INC_PC(sizeof(vfpinstr_inst));
+        INC_PC(sizeof(vmovbrrd_inst));
         FETCH_INST;
         GOTO_NEXT_INST;
 }
 #endif
-#ifdef VFP_MCRR_TRANS
-if (CoProc == 11 && (OPC_1 & 0xD) == 1)
-{
-        /* Transfering Rt and Rt2 is not mandatory, as the value of interest is pointed by value1 and value2 */
-        VMOVBRRD(state, BIT(20), Rt, Rt2, BIT(5)<<4|CRm, &value1, &value2);
-        return ARMul_DONE;
-}
-#endif
-#ifdef VFP_MRRC_TRANS
-if (CoProc == 11 && (OPC_1 & 0xD) == 1)
-{
-        /* Transfering Rt and Rt2 is not mandatory, as the value of interest is pointed by value1 and value2 */
-        VMOVBRRD(state, BIT(20), Rt, Rt2, BIT(5)<<4|CRm, value1, value2);
-        return ARMul_DONE;
-}
-#endif
-#ifdef VFP_MRRC_IMPL
-void VMOVBRRD(ARMul_State * state, ARMword to_arm, ARMword t, ARMword t2, ARMword n, ARMword *value1, ARMword *value2)
-{
-        DBG("VMOV(BRRD) :\n");
-        if (to_arm)
-        {
-                DBG("\tr[%d-%d] <= s[%d-%d]=[%x-%x]\n", t2, t, n*2+1, n*2, state->ExtReg[n*2+1], state->ExtReg[n*2]);
-                *value2 = state->ExtReg[n*2+1];
-                *value1 = state->ExtReg[n*2];
-        }
-        else
-        {
-                DBG("\ts[%d-%d] <= r[%d-%d]=[%x-%x]\n", n*2+1, n*2, t2, t, *value2, *value1);
-                state->ExtReg[n*2+1] = *value2;
-                state->ExtReg[n*2] = *value1;
-        }
-}
 
-#endif
 #ifdef VFP_DYNCOM_TABLE
-DYNCOM_FILL_ACTION(vfpinstr),
+DYNCOM_FILL_ACTION(vmovbrrd),
 #endif
 #ifdef VFP_DYNCOM_TAG
-int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
+int DYNCOM_TAG(vmovbrrd)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
 {
         int instr_size = INSTR_SIZE;
         //DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
@@ -3615,7 +2808,7 @@ int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr
 }
 #endif
 #ifdef VFP_DYNCOM_TRANS
-int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
+int DYNCOM_TRANS(vmovbrrd)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
         //DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
         //arch_arm_undef(cpu, bb, instr);
         int to_arm   = BIT(20) == 1;
@@ -3633,9 +2826,6 @@ int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc
         return No_exp;
 }
 #endif
-#undef vfpinstr
-#undef vfpinstr_inst
-#undef VFPLABEL_INST
 
 /* ----------------------------------------------------------------------- */
 /* LDC/STC between 2 registers and 1 double */
@@ -3645,21 +2835,6 @@ int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc
 /* ----------------------------------------------------------------------- */
 /* VSTR */
 /* cond 1101 UD00 Rn-- Vd-- 101X imm8 imm8 */
-#define vfpinstr        vstr
-#define vfpinstr_inst   vstr_inst
-#define VFPLABEL_INST   VSTR_INST
-#ifdef VFP_DECODE
-{"vstr",        3,      ARMVFP2,        24, 27, 0xd,   20, 21, 0,       9, 11, 0x5},
-#endif
-#ifdef VFP_DECODE_EXCLUSION
-{"vstr",    0,    ARMVFP2,    0},
-#endif
-#ifdef VFP_INTERPRETER_TABLE
-INTERPRETER_TRANSLATE(vfpinstr),
-#endif
-#ifdef VFP_INTERPRETER_LABEL
-&&VFPLABEL_INST,
-#endif
 #ifdef VFP_INTERPRETER_STRUCT
 typedef struct _vstr_inst {
         unsigned int single;
@@ -3667,15 +2842,15 @@ typedef struct _vstr_inst {
         unsigned int d;
         unsigned int imm32;
         unsigned int add;
-} vfpinstr_inst;
+} vstr_inst;
 #endif
 #ifdef VFP_INTERPRETER_TRANS
-ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
+ARM_INST_PTR INTERPRETER_TRANSLATE(vstr)(unsigned int inst, int index)
 {
         VFP_DEBUG_TRANSLATE;
         
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vfpinstr_inst));
-        vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vstr_inst));
+        vstr_inst *inst_cream = (vstr_inst *)inst_base->component;
 
         inst_base->cond  = BITS(inst, 28, 31);
         inst_base->idx   = index;
@@ -3692,13 +2867,12 @@ ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
 }
 #endif
 #ifdef VFP_INTERPRETER_IMPL
-VFPLABEL_INST:
+VSTR_INST:
 {
-        INC_ICOUNTER;
         if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
                 CHECK_VFP_ENABLED;
                 
-                vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+                vstr_inst *inst_cream = (vstr_inst *)inst_base->component;
                 
                 unsigned int base = (inst_cream->n == 15 ? (cpu->Reg[inst_cream->n] & 0xFFFFFFFC) + 8 : cpu->Reg[inst_cream->n]);
                 addr = (inst_cream->add ? base + inst_cream->imm32 : base - inst_cream->imm32);
@@ -3736,65 +2910,12 @@ VFPLABEL_INST:
         GOTO_NEXT_INST;
 }
 #endif
-#ifdef VFP_STC_TRANS
-if (P == 1 && W == 0)
-{
-        return VSTR(state, type, instr, value);
-}
-#endif
-#ifdef VFP_STC_IMPL
-int VSTR(ARMul_State * state, int type, ARMword instr, ARMword * value)
-{
-        static int i = 0;
-        static int single_reg, add, d, n, imm32, regs;
-        if (type == ARMul_FIRST)
-        {
-                single_reg = BIT(8) == 0;       /* Double precision */
-                add = BIT(23);          /* */
-                imm32 = BITS(0,7)<<2;   /* may not be used */
-                d = single_reg ? BITS(12, 15)<<1|BIT(22) : BIT(22)<<4|BITS(12, 15); /* Base register */
-                n = BITS(16, 19);       /* destination register */
-                
-                DBG("VSTR :\n");
-                
-                i = 0;
-                regs = 1;
-                
-                return ARMul_DONE;
-        }
-        else if (type == ARMul_DATA)
-        {
-                if (single_reg)
-                {
-                        *value = state->ExtReg[d+i];
-                        DBG("\taddr[?] <= s%d=[%x]\n", d+i, state->ExtReg[d+i]);
-                        i++;
-                        if (i < regs)
-                                return ARMul_INC;
-                        else
-                                return ARMul_DONE;
-                }
-                else
-                {
-                        /* FIXME Careful of endianness, may need to rework this */
-                        *value = state->ExtReg[d*2+i];
-                        DBG("\taddr[?] <= s[%d]=[%x]\n", d*2+i, state->ExtReg[d*2+i]);
-                        i++;
-                        if (i < regs*2)
-                                return ARMul_INC;
-                        else
-                                return ARMul_DONE;
-                }
-        }
 
-        return -1;
-}
-#endif
 #ifdef VFP_DYNCOM_TABLE
-DYNCOM_FILL_ACTION(vfpinstr),
+DYNCOM_FILL_ACTION(vstr),
 #endif
 #ifdef VFP_DYNCOM_TAG
-int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
+int DYNCOM_TAG(vstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
 {
         int instr_size = INSTR_SIZE;
         arm_tag_continue(cpu, pc, instr, tag, new_pc, next_pc);
@@ -3807,7 +2928,7 @@ int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr
 }
 #endif
 #ifdef VFP_DYNCOM_TRANS
-int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
+int DYNCOM_TRANS(vstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
         int single = BIT(8) == 0;
         int add    = BIT(23);
         int imm32  = BITS(0,7) << 2;
@@ -3853,43 +2974,25 @@ int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc
         return No_exp;
 }
 #endif
-#undef vfpinstr
-#undef vfpinstr_inst
-#undef VFPLABEL_INST
 
 /* ----------------------------------------------------------------------- */
 /* VPUSH */
 /* cond 1101 0D10 1101 Vd-- 101X imm8 imm8 */
-#define vfpinstr        vpush
-#define vfpinstr_inst   vpush_inst
-#define VFPLABEL_INST   VPUSH_INST
-#ifdef VFP_DECODE
-{"vpush",       3,      ARMVFP2,        23, 27, 0x1a,  16, 21, 0x2d,    9, 11, 0x5},
-#endif
-#ifdef VFP_DECODE_EXCLUSION
-{"vpush",    0,    ARMVFP2,    0},
-#endif
-#ifdef VFP_INTERPRETER_TABLE
-INTERPRETER_TRANSLATE(vfpinstr),
-#endif
-#ifdef VFP_INTERPRETER_LABEL
-&&VFPLABEL_INST,
-#endif
 #ifdef VFP_INTERPRETER_STRUCT
 typedef struct _vpush_inst {
         unsigned int single;
         unsigned int d;
         unsigned int imm32;
         unsigned int regs;
-} vfpinstr_inst;
+} vpush_inst;
 #endif
 #ifdef VFP_INTERPRETER_TRANS
-ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
+ARM_INST_PTR INTERPRETER_TRANSLATE(vpush)(unsigned int inst, int index)
 {
         VFP_DEBUG_TRANSLATE;
         
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vfpinstr_inst));
-        vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vpush_inst));
+        vpush_inst *inst_cream = (vpush_inst *)inst_base->component;
 
         inst_base->cond  = BITS(inst, 28, 31);
         inst_base->idx     = index;
@@ -3905,15 +3008,14 @@ ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
 }
 #endif
 #ifdef VFP_INTERPRETER_IMPL
-VFPLABEL_INST:
+VPUSH_INST:
 {
-        INC_ICOUNTER;
         if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
                 CHECK_VFP_ENABLED;
                                 
                 int i;
 
-                vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+                vpush_inst *inst_cream = (vpush_inst *)inst_base->component;
 
                 DBG("VPUSH :\n");
                         
@@ -3958,66 +3060,11 @@ VFPLABEL_INST:
         GOTO_NEXT_INST;
 }
 #endif
-#ifdef VFP_STC_TRANS
-if (P == 1 && U == 0 && W == 1 && Rn == 0xD)
-{
-        return VPUSH(state, type, instr, value);
-}
-#endif
-#ifdef VFP_STC_IMPL
-int VPUSH(ARMul_State * state, int type, ARMword instr, ARMword * value)
-{
-        static int i = 0;
-        static int single_regs, add, wback, d, n, imm32, regs;
-        if (type == ARMul_FIRST)
-        {
-                single_regs = BIT(8) == 0;      /* Single precision */
-                d = single_regs ? BITS(12, 15)<<1|BIT(22) : BIT(22)<<4|BITS(12, 15); /* Base register */
-                imm32 = BITS(0,7)<<2;   /* may not be used */
-                regs = single_regs ? BITS(0, 7) : BITS(1, 7); /* FSTMX if regs is odd */
-
-                DBG("VPUSH :\n");
-                DBG("\tsp[%x]", state->Reg[R13]);
-                state->Reg[R13] = state->Reg[R13] - imm32;
-                DBG("=>[%x]\n", state->Reg[R13]);
-                
-                i = 0;
-                
-                return ARMul_DONE;
-        } 
-        else if (type == ARMul_DATA)
-        {
-                if (single_regs)
-                {
-                        *value = state->ExtReg[d + i];
-                        DBG("\taddr[?] <= s%d=[%x]\n", d+i, state->ExtReg[d + i]);
-                        i++;
-                        if (i < regs)
-                                return ARMul_INC;
-                        else
-                                return ARMul_DONE;
-                }
-                else
-                {
-                        /* FIXME Careful of endianness, may need to rework this */
-                        *value = state->ExtReg[d*2 + i];
-                        DBG("\taddr[?] <= s[%d]=[%x]\n", d*2 + i, state->ExtReg[d*2 + i]);
-                        i++;
-                        if (i < regs*2)
-                                return ARMul_INC;
-                        else
-                                return ARMul_DONE;
-                }
-        }
-
-        return -1;
-}
-#endif
 #ifdef VFP_DYNCOM_TABLE
-DYNCOM_FILL_ACTION(vfpinstr),
+DYNCOM_FILL_ACTION(vpush),
 #endif
 #ifdef VFP_DYNCOM_TAG
-int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
+int DYNCOM_TAG(vpush)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
 {
         int instr_size = INSTR_SIZE;
         arm_tag_continue(cpu, pc, instr, tag, new_pc, next_pc);
@@ -4030,7 +3077,7 @@ int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr
 }
 #endif
 #ifdef VFP_DYNCOM_TRANS
-int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
+int DYNCOM_TRANS(vpush)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
         int single  = BIT(8) == 0;
         int d       = (single ? BITS(12, 15)<<1|BIT(22) : BITS(12, 15)|(BIT(22)<<4));
         int imm32   = BITS(0, 7)<<2;
@@ -4087,28 +3134,10 @@ int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc
         return No_exp;
 }
 #endif
-#undef vfpinstr
-#undef vfpinstr_inst
-#undef VFPLABEL_INST
 
 /* ----------------------------------------------------------------------- */
 /* VSTM */
 /* cond 110P UDW0 Rn-- Vd-- 101X imm8 imm8 */
-#define vfpinstr        vstm
-#define vfpinstr_inst   vstm_inst
-#define VFPLABEL_INST   VSTM_INST
-#ifdef VFP_DECODE
-{"vstm",        3,      ARMVFP2,        25, 27, 0x6,    20, 20, 0,      9, 11, 0x5},
-#endif
-#ifdef VFP_DECODE_EXCLUSION
-{"vstm",        0,      ARMVFP2,        0},
-#endif
-#ifdef VFP_INTERPRETER_TABLE
-INTERPRETER_TRANSLATE(vfpinstr),
-#endif
-#ifdef VFP_INTERPRETER_LABEL
-&&VFPLABEL_INST,
-#endif
 #ifdef VFP_INTERPRETER_STRUCT
 typedef struct _vstm_inst {
         unsigned int single;
@@ -4118,15 +3147,15 @@ typedef struct _vstm_inst {
         unsigned int n;
         unsigned int imm32;
         unsigned int regs;
-} vfpinstr_inst;
+} vstm_inst;
 #endif
 #ifdef VFP_INTERPRETER_TRANS
-ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
+ARM_INST_PTR INTERPRETER_TRANSLATE(vstm)(unsigned int inst, int index)
 {
         VFP_DEBUG_TRANSLATE;
         
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vfpinstr_inst));
-        vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vstm_inst));
+        vstm_inst *inst_cream = (vstm_inst *)inst_base->component;
 
         inst_base->cond  = BITS(inst, 28, 31);
         inst_base->idx     = index;
@@ -4145,15 +3174,14 @@ ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
 }
 #endif
 #ifdef VFP_INTERPRETER_IMPL
-VFPLABEL_INST: /* encoding 1 */
+VSTM_INST: /* encoding 1 */
 {
-        INC_ICOUNTER;
         if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
                 CHECK_VFP_ENABLED;
                 
                 int i;
                 
-                vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+                vstm_inst *inst_cream = (vstm_inst *)inst_base->component;
                 
                 addr = (inst_cream->add ? cpu->Reg[inst_cream->n] : cpu->Reg[inst_cream->n] - inst_cream->imm32);
                 DBG("VSTM : addr[%x]\n", addr);
@@ -4203,69 +3231,12 @@ VFPLABEL_INST: /* encoding 1 */
         GOTO_NEXT_INST;
 }
 #endif
-#ifdef VFP_STC_TRANS
-/* Should be the last operation of STC */
-return VSTM(state, type, instr, value);
-#endif
-#ifdef VFP_STC_IMPL
-int VSTM(ARMul_State * state, int type, ARMword instr, ARMword * value)
-{
-        static int i = 0;
-        static int single_regs, add, wback, d, n, imm32, regs;
-        if (type == ARMul_FIRST)
-        {
-                single_regs = BIT(8) == 0;      /* Single precision */
-                add = BIT(23);          /* */
-                wback = BIT(21);        /* write-back */
-                d = single_regs ? BITS(12, 15)<<1|BIT(22) : BIT(22)<<4|BITS(12, 15); /* Base register */
-                n = BITS(16, 19);       /* destination register */
-                imm32 = BITS(0,7) * 4;  /* may not be used */
-                regs = single_regs ? BITS(0, 7) : BITS(0, 7)>>1; /* FSTMX if regs is odd */
-
-                DBG("VSTM :\n");
-                
-                if (wback) {
-                        state->Reg[n] = (add ? state->Reg[n] + imm32 : state->Reg[n] - imm32);
-                        DBG("\twback r%d[%x]\n", n, state->Reg[n]);
-                }
-                
-                i = 0;
-                
-                return ARMul_DONE;
-        } 
-        else if (type == ARMul_DATA)
-        {
-                if (single_regs)
-                {
-                        *value = state->ExtReg[d + i];
-                        DBG("\taddr[?] <= s%d=[%x]\n", d+i, state->ExtReg[d + i]);
-                        i++;
-                        if (i < regs)
-                                return ARMul_INC;
-                        else
-                                return ARMul_DONE;
-                }
-                else
-                {
-                        /* FIXME Careful of endianness, may need to rework this */
-                        *value = state->ExtReg[d*2 + i];
-                        DBG("\taddr[?] <= s[%d]=[%x]\n", d*2 + i, state->ExtReg[d*2 + i]);
-                        i++;
-                        if (i < regs*2)
-                                return ARMul_INC;
-                        else
-                                return ARMul_DONE;
-                }
-        }
 
-        return -1;
-}
-#endif
 #ifdef VFP_DYNCOM_TABLE
-DYNCOM_FILL_ACTION(vfpinstr),
+DYNCOM_FILL_ACTION(vstm),
 #endif
 #ifdef VFP_DYNCOM_TAG
-int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
+int DYNCOM_TAG(vstm)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
 {
         int instr_size = INSTR_SIZE;
         //DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
@@ -4280,7 +3251,7 @@ int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr
 }
 #endif
 #ifdef VFP_DYNCOM_TRANS
-int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
+int DYNCOM_TRANS(vstm)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
         //arch_arm_undef(cpu, bb, instr);
         int single = BIT(8) == 0;
         int add    = BIT(23);
@@ -4356,43 +3327,25 @@ int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc
         return No_exp;
 }
 #endif
-#undef vfpinstr
-#undef vfpinstr_inst
-#undef VFPLABEL_INST
 
 /* ----------------------------------------------------------------------- */
 /* VPOP */
 /* cond 1100 1D11 1101 Vd-- 101X imm8 imm8 */
-#define vfpinstr        vpop
-#define vfpinstr_inst   vpop_inst
-#define VFPLABEL_INST   VPOP_INST
-#ifdef VFP_DECODE
-{"vpop",        3,      ARMVFP2,        23, 27, 0x19,  16, 21, 0x3d,    9, 11, 0x5},
-#endif
-#ifdef VFP_DECODE_EXCLUSION
-{"vpop",    0,    ARMVFP2,    0},
-#endif
-#ifdef VFP_INTERPRETER_TABLE
-INTERPRETER_TRANSLATE(vfpinstr),
-#endif
-#ifdef VFP_INTERPRETER_LABEL
-&&VFPLABEL_INST,
-#endif
 #ifdef VFP_INTERPRETER_STRUCT
 typedef struct _vpop_inst {
         unsigned int single;
         unsigned int d;
         unsigned int imm32;
         unsigned int regs;
-} vfpinstr_inst;
+} vpop_inst;
 #endif
 #ifdef VFP_INTERPRETER_TRANS
-ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
+ARM_INST_PTR INTERPRETER_TRANSLATE(vpop)(unsigned int inst, int index)
 {
         VFP_DEBUG_TRANSLATE;
         
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vfpinstr_inst));
-        vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vpop_inst));
+        vpop_inst *inst_cream = (vpop_inst *)inst_base->component;
 
         inst_base->cond  = BITS(inst, 28, 31);
         inst_base->idx   = index;
@@ -4408,16 +3361,15 @@ ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
 }
 #endif
 #ifdef VFP_INTERPRETER_IMPL
-VFPLABEL_INST:
+VPOP_INST:
 {
-        INC_ICOUNTER;
         if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
                 CHECK_VFP_ENABLED;
                 
                 int i;
                 unsigned int value1, value2;
 
-                vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+                vpop_inst *inst_cream = (vpop_inst *)inst_base->component;
                 
                 DBG("VPOP :\n");
                 
@@ -4468,70 +3420,12 @@ VFPLABEL_INST:
         GOTO_NEXT_INST;
 }
 #endif
-#ifdef VFP_LDC_TRANS
-if (P == 0 && U == 1 && W == 1 && Rn == 0xD)
-{
-        return VPOP(state, type, instr, value);
-}
-#endif
-#ifdef VFP_LDC_IMPL
-int VPOP(ARMul_State * state, int type, ARMword instr, ARMword value)
-{
-        static int i = 0;
-        static int single_regs, add, wback, d, n, imm32, regs;
-        if (type == ARMul_FIRST)
-        {
-                single_regs = BIT(8) == 0;      /* Single precision */
-                d = single_regs ? BITS(12, 15)<<1|BIT(22) : BIT(22)<<4|BITS(12, 15); /* Base register */
-                imm32 = BITS(0,7)<<2;   /* may not be used */
-                regs = single_regs ? BITS(0, 7) : BITS(1, 7); /* FLDMX if regs is odd */
 
-                DBG("VPOP :\n");
-                DBG("\tsp[%x]", state->Reg[R13]);
-                state->Reg[R13] = state->Reg[R13] + imm32;
-                DBG("=>[%x]\n", state->Reg[R13]);
-                
-                i = 0;
-                
-                return ARMul_DONE;
-        }
-        else if (type == ARMul_TRANSFER)
-        {
-                return ARMul_DONE;
-        }
-        else if (type == ARMul_DATA)
-        {
-                if (single_regs)
-                {
-                        state->ExtReg[d + i] = value;
-                        DBG("\ts%d <= [%x]\n", d + i, value);
-                        i++;
-                        if (i < regs)
-                                return ARMul_INC;
-                        else
-                                return ARMul_DONE;
-                }
-                else
-                {
-                        /* FIXME Careful of endianness, may need to rework this */
-                        state->ExtReg[d*2 + i] = value;
-                        DBG("\ts%d <= [%x]\n", d*2 + i, value);
-                        i++;
-                        if (i < regs*2)
-                                return ARMul_INC;
-                        else
-                                return ARMul_DONE;
-                }
-        }
-
-        return -1;
-}
-#endif
 #ifdef VFP_DYNCOM_TABLE
-DYNCOM_FILL_ACTION(vfpinstr),
+DYNCOM_FILL_ACTION(vpop),
 #endif
 #ifdef VFP_DYNCOM_TAG
-int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
+int DYNCOM_TAG(vpop)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
 {
         int instr_size = INSTR_SIZE;
         //DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
@@ -4547,7 +3441,7 @@ int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr
 }
 #endif
 #ifdef VFP_DYNCOM_TRANS
-int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
+int DYNCOM_TRANS(vpop)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
         DBG("\t\tin %s instruction .\n", __FUNCTION__);
         //arch_arm_undef(cpu, bb, instr);
         int single  = BIT(8) == 0;
@@ -4611,28 +3505,10 @@ int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc
         return No_exp;
 }
 #endif
-#undef vfpinstr
-#undef vfpinstr_inst
-#undef VFPLABEL_INST
 
 /* ----------------------------------------------------------------------- */
 /* VLDR */
 /* cond 1101 UD01 Rn-- Vd-- 101X imm8 imm8 */
-#define vfpinstr        vldr
-#define vfpinstr_inst   vldr_inst
-#define VFPLABEL_INST   VLDR_INST
-#ifdef VFP_DECODE
-{"vldr",        3,      ARMVFP2,        24, 27, 0xd,   20, 21, 0x1,     9, 11, 0x5},
-#endif
-#ifdef VFP_DECODE_EXCLUSION
-{"vldr",    0,    ARMVFP2,    0},
-#endif
-#ifdef VFP_INTERPRETER_TABLE
-INTERPRETER_TRANSLATE(vfpinstr),
-#endif
-#ifdef VFP_INTERPRETER_LABEL
-&&VFPLABEL_INST,
-#endif
 #ifdef VFP_INTERPRETER_STRUCT
 typedef struct _vldr_inst {
         unsigned int single;
@@ -4640,15 +3516,15 @@ typedef struct _vldr_inst {
         unsigned int d;
         unsigned int imm32;
         unsigned int add;
-} vfpinstr_inst;
+} vldr_inst;
 #endif
 #ifdef VFP_INTERPRETER_TRANS
-ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
+ARM_INST_PTR INTERPRETER_TRANSLATE(vldr)(unsigned int inst, int index)
 {
         VFP_DEBUG_TRANSLATE;
         
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vfpinstr_inst));
-        vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vldr_inst));
+        vldr_inst *inst_cream = (vldr_inst *)inst_base->component;
 
         inst_base->cond  = BITS(inst, 28, 31);
         inst_base->idx   = index;
@@ -4665,13 +3541,12 @@ ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
 }
 #endif
 #ifdef VFP_INTERPRETER_IMPL
-VFPLABEL_INST:
+VLDR_INST:
 {
-        INC_ICOUNTER;
         if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
                 CHECK_VFP_ENABLED;
                 
-                vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+                vldr_inst *inst_cream = (vldr_inst *)inst_base->component;
                 
                 unsigned int base = (inst_cream->n == 15 ? (cpu->Reg[inst_cream->n] & 0xFFFFFFFC) + 8 : cpu->Reg[inst_cream->n]);
                 addr = (inst_cream->add ? base + inst_cream->imm32 : base - inst_cream->imm32);
@@ -4710,69 +3585,12 @@ VFPLABEL_INST:
         GOTO_NEXT_INST;
 }
 #endif
-#ifdef VFP_LDC_TRANS
-if (P == 1 && W == 0)
-{
-        return VLDR(state, type, instr, value);
-}
-#endif
-#ifdef VFP_LDC_IMPL
-int VLDR(ARMul_State * state, int type, ARMword instr, ARMword value)
-{
-        static int i = 0;
-        static int single_reg, add, d, n, imm32, regs;
-        if (type == ARMul_FIRST)
-        {
-                single_reg = BIT(8) == 0;       /* Double precision */
-                add = BIT(23);          /* */
-                imm32 = BITS(0,7)<<2;   /* may not be used */
-                d = single_reg ? BITS(12, 15)<<1|BIT(22) : BIT(22)<<4|BITS(12, 15); /* Base register */
-                n = BITS(16, 19);       /* destination register */
-                
-                DBG("VLDR :\n");
-                
-                i = 0;
-                regs = 1;
-                
-                return ARMul_DONE;
-        }
-        else if (type == ARMul_TRANSFER)
-        {
-                return ARMul_DONE;
-        }
-        else if (type == ARMul_DATA)
-        {
-                if (single_reg)
-                {
-                        state->ExtReg[d+i] = value;
-                        DBG("\ts%d <= [%x]\n", d+i, value);
-                        i++;
-                        if (i < regs)
-                                return ARMul_INC;
-                        else
-                                return ARMul_DONE;
-                }
-                else
-                {
-                        /* FIXME Careful of endianness, may need to rework this */
-                        state->ExtReg[d*2+i] = value;
-                        DBG("\ts[%d] <= [%x]\n", d*2+i, value);
-                        i++;
-                        if (i < regs*2)
-                                return ARMul_INC;
-                        else
-                                return ARMul_DONE;
-                }
-        }
 
-        return -1;
-}
-#endif
 #ifdef VFP_DYNCOM_TABLE
-DYNCOM_FILL_ACTION(vfpinstr),
+DYNCOM_FILL_ACTION(vldr),
 #endif
 #ifdef VFP_DYNCOM_TAG
-int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
+int DYNCOM_TAG(vldr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
 {
         int instr_size = INSTR_SIZE;
         //DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
@@ -4788,7 +3606,7 @@ int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr
 }
 #endif
 #ifdef VFP_DYNCOM_TRANS
-int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
+int DYNCOM_TRANS(vldr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
         int single = BIT(8) == 0;
         int add    = BIT(23);
         int wback  = BIT(21);
@@ -4846,28 +3664,10 @@ int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc
         return No_exp;
 }
 #endif
-#undef vfpinstr
-#undef vfpinstr_inst
-#undef VFPLABEL_INST
 
 /* ----------------------------------------------------------------------- */
 /* VLDM */
 /* cond 110P UDW1 Rn-- Vd-- 101X imm8 imm8 */
-#define vfpinstr        vldm
-#define vfpinstr_inst   vldm_inst
-#define VFPLABEL_INST   VLDM_INST
-#ifdef VFP_DECODE
-{"vldm",        3,      ARMVFP2,        25, 27, 0x6,    20, 20, 1,      9, 11, 0x5},
-#endif
-#ifdef VFP_DECODE_EXCLUSION
-{"vldm",        0,      ARMVFP2,        0},
-#endif
-#ifdef VFP_INTERPRETER_TABLE
-INTERPRETER_TRANSLATE(vfpinstr),
-#endif
-#ifdef VFP_INTERPRETER_LABEL
-&&VFPLABEL_INST,
-#endif
 #ifdef VFP_INTERPRETER_STRUCT
 typedef struct _vldm_inst {
         unsigned int single;
@@ -4877,15 +3677,15 @@ typedef struct _vldm_inst {
         unsigned int n;
         unsigned int imm32;
         unsigned int regs;
-} vfpinstr_inst;
+} vldm_inst;
 #endif
 #ifdef VFP_INTERPRETER_TRANS
-ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
+ARM_INST_PTR INTERPRETER_TRANSLATE(vldm)(unsigned int inst, int index)
 {
         VFP_DEBUG_TRANSLATE;
         
-        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vfpinstr_inst));
-        vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+        arm_inst *inst_base = (arm_inst *)AllocBuffer(sizeof(arm_inst) + sizeof(vldm_inst));
+        vldm_inst *inst_cream = (vldm_inst *)inst_base->component;
 
         inst_base->cond  = BITS(inst, 28, 31);
         inst_base->idx     = index;
@@ -4904,15 +3704,14 @@ ARM_INST_PTR INTERPRETER_TRANSLATE(vfpinstr)(unsigned int inst, int index)
 }
 #endif
 #ifdef VFP_INTERPRETER_IMPL
-VFPLABEL_INST:
+VLDM_INST:
 {
-        INC_ICOUNTER;
         if ((inst_base->cond == 0xe) || CondPassed(cpu, inst_base->cond)) {
                 CHECK_VFP_ENABLED;
                 
                 int i;
                 
-                vfpinstr_inst *inst_cream = (vfpinstr_inst *)inst_base->component;
+                vldm_inst *inst_cream = (vldm_inst *)inst_base->component;
                 
                 addr = (inst_cream->add ? cpu->Reg[inst_cream->n] : cpu->Reg[inst_cream->n] - inst_cream->imm32);
                 DBG("VLDM : addr[%x]\n", addr);
@@ -4952,74 +3751,17 @@ VFPLABEL_INST:
 
         }
         cpu->Reg[15] += GET_INST_SIZE(cpu);
-        INC_PC(sizeof(vfpinstr_inst));
+        INC_PC(sizeof(vldm_inst));
         FETCH_INST;
         GOTO_NEXT_INST;
 }
 #endif
-#ifdef VFP_LDC_TRANS
-/* Should be the last operation of LDC */
-return VLDM(state, type, instr, value);
-#endif
-#ifdef VFP_LDC_IMPL
-int VLDM(ARMul_State * state, int type, ARMword instr, ARMword value)
-{
-        static int i = 0;
-        static int single_regs, add, wback, d, n, imm32, regs;
-        if (type == ARMul_FIRST)
-        {
-                single_regs = BIT(8) == 0;      /* Single precision */
-                add = BIT(23);          /* */
-                wback = BIT(21);        /* write-back */
-                d = single_regs ? BITS(12, 15)<<1|BIT(22) : BIT(22)<<4|BITS(12, 15); /* Base register */
-                n = BITS(16, 19);       /* destination register */
-                imm32 = BITS(0,7) * 4;  /* may not be used */
-                regs = single_regs ? BITS(0, 7) : BITS(0, 7)>>1; /* FLDMX if regs is odd */
-
-                DBG("VLDM :\n");
-                
-                if (wback) {
-                        state->Reg[n] = (add ? state->Reg[n] + imm32 : state->Reg[n] - imm32);
-                        DBG("\twback r%d[%x]\n", n, state->Reg[n]);
-                }
-                
-                i = 0;
-                
-                return ARMul_DONE;
-        } 
-        else if (type == ARMul_DATA)
-        {
-                if (single_regs)
-                {
-                        state->ExtReg[d + i] = value;
-                        DBG("\ts%d <= [%x] addr[?]\n", d+i, state->ExtReg[d + i]);
-                        i++;
-                        if (i < regs)
-                                return ARMul_INC;
-                        else
-                                return ARMul_DONE;
-                }
-                else
-                {
-                        /* FIXME Careful of endianness, may need to rework this */
-                        state->ExtReg[d*2 + i] = value;
-                        DBG("\ts[%d] <= [%x] addr[?]\n", d*2 + i, state->ExtReg[d*2 + i]);
-                        i++;
-                        if (i < regs*2)
-                                return ARMul_INC;
-                        else
-                                return ARMul_DONE;
-                }
-        }
 
-        return -1;
-}
-#endif
 #ifdef VFP_DYNCOM_TABLE
-DYNCOM_FILL_ACTION(vfpinstr),
+DYNCOM_FILL_ACTION(vldm),
 #endif
 #ifdef VFP_DYNCOM_TAG
-int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
+int DYNCOM_TAG(vldm)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr_t *new_pc, addr_t *next_pc)
 {
         int instr_size = INSTR_SIZE;
         //DBG("\t\tin %s instruction is not implemented.\n", __FUNCTION__);
@@ -5034,7 +3776,7 @@ int DYNCOM_TAG(vfpinstr)(cpu_t *cpu, addr_t pc, uint32_t instr, tag_t *tag, addr
 }
 #endif
 #ifdef VFP_DYNCOM_TRANS
-int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
+int DYNCOM_TRANS(vldm)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc){
         int single = BIT(8) == 0;
         int add    = BIT(23);
         int wback  = BIT(21);
@@ -5110,14 +3852,3 @@ int DYNCOM_TRANS(vfpinstr)(cpu_t *cpu, uint32_t instr, BasicBlock *bb, addr_t pc
         return No_exp;
 }
 #endif
-#undef vfpinstr
-#undef vfpinstr_inst
-#undef VFPLABEL_INST
-
-#define VFP_DEBUG_TRANSLATE DBG("in func %s, %x\n", __FUNCTION__, inst);
-#define VFP_DEBUG_UNIMPLEMENTED(x) printf("in func %s, " #x " unimplemented\n", __FUNCTION__); exit(-1);
-#define VFP_DEBUG_UNTESTED(x) printf("in func %s, " #x " untested\n", __FUNCTION__);
-
-#define CHECK_VFP_ENABLED       
-        
-#define CHECK_VFP_CDP_RET       vfp_raise_exceptions(cpu, ret, inst_cream->instr, cpu->VFP[VFP_OFFSET(VFP_FPSCR)]); //if (ret == -1) {printf("VFP CDP FAILURE %x\n", inst_cream->instr); exit(-1);}
diff --git a/src/core/arm/skyeye_common/vfp/vfpsingle.cpp b/src/core/arm/skyeye_common/vfp/vfpsingle.cpp
index 07d0c1f44..6c33d8b78 100644
--- a/src/core/arm/skyeye_common/vfp/vfpsingle.cpp
+++ b/src/core/arm/skyeye_common/vfp/vfpsingle.cpp
@@ -522,8 +522,7 @@ static s64 vfp_single_to_doubleintern(ARMul_State* state, s32 m, u32 fpscr) //ic
         if (tm == VFP_QNAN)
             vdd.significand |= VFP_DOUBLE_SIGNIFICAND_QNAN;
         goto pack_nan;
-    }
-    else if (tm & VFP_ZERO)
+    } else if (tm & VFP_ZERO)
         vdd.exponent = 0;
     else
         vdd.exponent = vsm.exponent + (1023 - 127);
@@ -615,12 +614,12 @@ static u32 vfp_single_ftoui(ARMul_State* state, int sd, int unused, s32 m, u32 f
         exceptions |= FPSCR_IDC;
 
     if (tm & VFP_NAN)
-        vsm.sign = 0;
+        vsm.sign = 1;
 
     if (vsm.exponent >= 127 + 32) {
         d = vsm.sign ? 0 : 0xffffffff;
         exceptions = FPSCR_IOC;
-    } else if (vsm.exponent >= 127 - 1) {
+    } else if (vsm.exponent >= 127) {
         int shift = 127 + 31 - vsm.exponent;
         u32 rem, incr = 0;
 
@@ -705,7 +704,7 @@ static u32 vfp_single_ftosi(ARMul_State* state, int sd, int unused, s32 m, u32 f
         if (vsm.sign)
             d = ~d;
         exceptions |= FPSCR_IOC;
-    } else if (vsm.exponent >= 127 - 1) {
+    } else if (vsm.exponent >= 127) {
         int shift = 127 + 31 - vsm.exponent;
         u32 rem, incr = 0;
 
@@ -1149,7 +1148,10 @@ static u32 vfp_single_fsub(ARMul_State* state, int sd, int sn, s32 m, u32 fpscr)
     /*
      * Subtraction is addition with one sign inverted.
      */
-    return vfp_single_fadd(state, sd, sn, vfp_single_packed_negate(m), fpscr);
+    if (m != 0x7FC00000) // Only negate if m isn't NaN.
+        m = vfp_single_packed_negate(m);
+
+    return vfp_single_fadd(state, sd, sn, m, fpscr);
 }
 
 /*
diff --git a/src/core/core.cpp b/src/core/core.cpp
index 64de0cbba..22213d647 100644
--- a/src/core/core.cpp
+++ b/src/core/core.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "common/common_types.h"
diff --git a/src/core/core.h b/src/core/core.h
index 850bb0ab4..05dbe0ae5 100644
--- a/src/core/core.h
+++ b/src/core/core.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/core/core_timing.cpp b/src/core/core_timing.cpp
index 1a0b2724a..321648b37 100644
--- a/src/core/core_timing.cpp
+++ b/src/core/core_timing.cpp
@@ -1,5 +1,5 @@
-// Copyright 2013 Dolphin Emulator Project
-// Licensed under GPLv2
+// Copyright 2013 Dolphin Emulator Project / 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include <vector>
diff --git a/src/core/core_timing.h b/src/core/core_timing.h
index b197cf40c..496234538 100644
--- a/src/core/core_timing.h
+++ b/src/core/core_timing.h
@@ -1,5 +1,5 @@
-// Copyright 2013 Dolphin Emulator Project
-// Licensed under GPLv2
+// Copyright 2013 Dolphin Emulator Project / 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/core/file_sys/archive.h b/src/core/file_sys/archive.h
deleted file mode 100644
index 27ed23cd0..000000000
--- a/src/core/file_sys/archive.h
+++ /dev/null
@@ -1,268 +0,0 @@
-// Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
-// Refer to the license.txt file included.
-
-#pragma once
-
-#include <memory>
-
-#include "common/common_types.h"
-#include "common/string_util.h"
-#include "common/bit_field.h"
-
-#include "core/file_sys/file.h"
-#include "core/file_sys/directory.h"
-
-#include "core/mem_map.h"
-#include "core/hle/kernel/kernel.h"
-
-////////////////////////////////////////////////////////////////////////////////////////////////////
-// FileSys namespace
-
-namespace FileSys {
-
-// Path string type
-enum LowPathType : u32 {
-    Invalid = 0,
-    Empty   = 1,
-    Binary  = 2,
-    Char    = 3,
-    Wchar   = 4
-};
-
-union Mode {
-    u32 hex;
-    BitField<0, 1, u32> read_flag;
-    BitField<1, 1, u32> write_flag;
-    BitField<2, 1, u32> create_flag;
-};
-
-class Path {
-public:
-
-    Path():
-        type(Invalid)
-    {
-    }
-
-    Path(LowPathType type, u32 size, u32 pointer):
-        type(type)
-    {
-        switch (type) {
-            case Binary:
-            {
-                u8* data = Memory::GetPointer(pointer);
-                binary = std::vector<u8>(data, data + size);
-                break;
-            }
-            case Char:
-            {
-                const char* data = reinterpret_cast<const char*>(Memory::GetPointer(pointer));
-                string = std::string(data, size - 1); // Data is always null-terminated.
-                break;
-            }
-            case Wchar:
-            {
-                const char16_t* data = reinterpret_cast<const char16_t*>(Memory::GetPointer(pointer));
-                u16str = std::u16string(data, size/2 - 1); // Data is always null-terminated.
-                break;
-            }
-            default:
-                break;
-        }
-    }
-
-    LowPathType GetType() const {
-        return type;
-    }
-
-    /**
-     * Gets the string representation of the path for debugging
-     * @return String representation of the path for debugging
-     */
-    const std::string DebugStr() const {
-        switch (GetType()) {
-        case Invalid:
-            return "[Invalid]";
-        case Empty:
-            return "[Empty]";
-        case Binary:
-        {
-            std::stringstream res;
-            res << "[Binary: ";
-            for (unsigned byte : binary)
-                res << std::hex << std::setw(2) << std::setfill('0') << byte;
-            res << ']';
-            return res.str();
-        }
-        case Char:
-            return "[Char: " + AsString() + ']';
-        case Wchar:
-            return "[Wchar: " + AsString() + ']';
-        default:
-            // TODO(yuriks): Add assert
-            LOG_ERROR(Service_FS, "LowPathType cannot be converted to string!");
-            return {};
-        }
-    }
-
-    const std::string AsString() const {
-        switch (GetType()) {
-            case Char:
-                return string;
-            case Wchar:
-                return Common::UTF16ToUTF8(u16str);
-            case Empty:
-                return {};
-            default:
-                // TODO(yuriks): Add assert
-                LOG_ERROR(Service_FS, "LowPathType cannot be converted to string!");
-                return {};
-        }
-    }
-
-    const std::u16string AsU16Str() const {
-        switch (GetType()) {
-            case Char:
-                return Common::UTF8ToUTF16(string);
-            case Wchar:
-                return u16str;
-            case Empty:
-                return {};
-            default:
-                // TODO(yuriks): Add assert
-                LOG_ERROR(Service_FS, "LowPathType cannot be converted to u16string!");
-                return {};
-        }
-    }
-
-    const std::vector<u8> AsBinary() const {
-        switch (GetType()) {
-            case Binary:
-                return binary;
-            case Char:
-                return std::vector<u8>(string.begin(), string.end());
-            case Wchar:
-                return std::vector<u8>(u16str.begin(), u16str.end());
-            case Empty:
-                return {};
-            default:
-                // TODO(yuriks): Add assert
-                LOG_ERROR(Service_FS, "LowPathType cannot be converted to binary!");
-                return {};
-        }
-    }
-
-private:
-    LowPathType type;
-    std::vector<u8> binary;
-    std::string string;
-    std::u16string u16str;
-};
-
-class Archive : NonCopyable {
-public:
-    /// Supported archive types
-    enum class IdCode : u32 {
-        RomFS               = 0x00000003,
-        SaveData            = 0x00000004,
-        ExtSaveData         = 0x00000006,
-        SharedExtSaveData   = 0x00000007,
-        SystemSaveData      = 0x00000008,
-        SDMC                = 0x00000009,
-        SDMCWriteOnly       = 0x0000000A,
-    };
-
-    Archive() { }
-    virtual ~Archive() { }
-
-    /**
-     * Get the IdCode of the archive (e.g. RomFS, SaveData, etc.)
-     * @return IdCode of the archive
-     */
-    virtual IdCode GetIdCode() const = 0;
-
-    /**
-     * Open a file specified by its path, using the specified mode
-     * @param path Path relative to the archive
-     * @param mode Mode to open the file with
-     * @return Opened file, or nullptr
-     */
-    virtual std::unique_ptr<File> OpenFile(const Path& path, const Mode mode) const = 0;
-
-    /**
-     * Delete a file specified by its path
-     * @param path Path relative to the archive
-     * @return Whether the file could be deleted
-     */
-    virtual bool DeleteFile(const FileSys::Path& path) const = 0;
-
-    /**
-     * Rename a File specified by its path
-     * @param src_path Source path relative to the archive
-     * @param dest_path Destination path relative to the archive
-     * @return Whether rename succeeded
-     */
-    virtual bool RenameFile(const FileSys::Path& src_path, const FileSys::Path& dest_path) const = 0;
-
-    /**
-     * Delete a directory specified by its path
-     * @param path Path relative to the archive
-     * @return Whether the directory could be deleted
-     */
-    virtual bool DeleteDirectory(const FileSys::Path& path) const = 0;
-
-    /**
-     * Create a directory specified by its path
-     * @param path Path relative to the archive
-     * @return Whether the directory could be created
-     */
-    virtual bool CreateDirectory(const Path& path) const = 0;
-
-    /**
-     * Rename a Directory specified by its path
-     * @param src_path Source path relative to the archive
-     * @param dest_path Destination path relative to the archive
-     * @return Whether rename succeeded
-     */
-    virtual bool RenameDirectory(const FileSys::Path& src_path, const FileSys::Path& dest_path) const = 0;
-
-    /**
-     * Open a directory specified by its path
-     * @param path Path relative to the archive
-     * @return Opened directory, or nullptr
-     */
-    virtual std::unique_ptr<Directory> OpenDirectory(const Path& path) const = 0;
-
-    /**
-     * Read data from the archive
-     * @param offset Offset in bytes to start reading data from
-     * @param length Length in bytes of data to read from archive
-     * @param buffer Buffer to read data into
-     * @return Number of bytes read
-     */
-    virtual size_t Read(const u64 offset, const u32 length, u8* buffer) const = 0;
-
-    /**
-     * Write data to the archive
-     * @param offset Offset in bytes to start writing data to
-     * @param length Length in bytes of data to write to archive
-     * @param buffer Buffer to write data from
-     * @param flush  The flush parameters (0 == do not flush)
-     * @return Number of bytes written
-     */
-    virtual size_t Write(const u64 offset, const u32 length, const u32 flush, u8* buffer) = 0;
-
-    /**
-     * Get the size of the archive in bytes
-     * @return Size of the archive in bytes
-     */
-    virtual size_t GetSize() const = 0;
-
-    /**
-     * Set the size of the archive in bytes
-     */
-    virtual void SetSize(const u64 size) = 0;
-};
-
-} // namespace FileSys
diff --git a/src/core/file_sys/archive_backend.h b/src/core/file_sys/archive_backend.h
new file mode 100644
index 000000000..e153917ea
--- /dev/null
+++ b/src/core/file_sys/archive_backend.h
@@ -0,0 +1,243 @@
+// Copyright 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#pragma once
+
+#include <memory>
+
+#include "common/common_types.h"
+#include "common/string_util.h"
+#include "common/bit_field.h"
+
+#include "core/file_sys/file_backend.h"
+#include "core/file_sys/directory_backend.h"
+
+#include "core/mem_map.h"
+#include "core/hle/kernel/kernel.h"
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// FileSys namespace
+
+namespace FileSys {
+
+// Path string type
+enum LowPathType : u32 {
+    Invalid = 0,
+    Empty   = 1,
+    Binary  = 2,
+    Char    = 3,
+    Wchar   = 4
+};
+
+union Mode {
+    u32 hex;
+    BitField<0, 1, u32> read_flag;
+    BitField<1, 1, u32> write_flag;
+    BitField<2, 1, u32> create_flag;
+};
+
+class Path {
+public:
+
+    Path() : type(Invalid) {
+    }
+
+    Path(const char* path) : type(Char), string(path) {
+    }
+
+    Path(LowPathType type, u32 size, u32 pointer) : type(type) {
+        switch (type) {
+        case Binary:
+        {
+            u8* data = Memory::GetPointer(pointer);
+            binary = std::vector<u8>(data, data + size);
+            break;
+        }
+
+        case Char:
+        {
+            const char* data = reinterpret_cast<const char*>(Memory::GetPointer(pointer));
+            string = std::string(data, size - 1); // Data is always null-terminated.
+            break;
+        }
+
+        case Wchar:
+        {
+            const char16_t* data = reinterpret_cast<const char16_t*>(Memory::GetPointer(pointer));
+            u16str = std::u16string(data, size/2 - 1); // Data is always null-terminated.
+            break;
+        }
+
+        default:
+            break;
+        }
+    }
+
+    LowPathType GetType() const {
+        return type;
+    }
+
+    /**
+     * Gets the string representation of the path for debugging
+     * @return String representation of the path for debugging
+     */
+    const std::string DebugStr() const {
+        switch (GetType()) {
+        case Invalid:
+            return "[Invalid]";
+        case Empty:
+            return "[Empty]";
+        case Binary:
+        {
+            std::stringstream res;
+            res << "[Binary: ";
+            for (unsigned byte : binary)
+                res << std::hex << std::setw(2) << std::setfill('0') << byte;
+            res << ']';
+            return res.str();
+        }
+        case Char:
+            return "[Char: " + AsString() + ']';
+        case Wchar:
+            return "[Wchar: " + AsString() + ']';
+        }
+    }
+
+    const std::string AsString() const {
+        switch (GetType()) {
+        case Char:
+            return string;
+        case Wchar:
+            return Common::UTF16ToUTF8(u16str);
+        case Empty:
+            return {};
+        case Invalid:
+        case Binary:
+            // TODO(yuriks): Add assert
+            LOG_ERROR(Service_FS, "LowPathType cannot be converted to string!");
+            return {};
+        }
+    }
+
+    const std::u16string AsU16Str() const {
+        switch (GetType()) {
+        case Char:
+            return Common::UTF8ToUTF16(string);
+        case Wchar:
+            return u16str;
+        case Empty:
+            return {};
+        case Invalid:
+        case Binary:
+            // TODO(yuriks): Add assert
+            LOG_ERROR(Service_FS, "LowPathType cannot be converted to u16string!");
+            return {};
+        }
+    }
+
+    const std::vector<u8> AsBinary() const {
+        switch (GetType()) {
+        case Binary:
+            return binary;
+        case Char:
+            return std::vector<u8>(string.begin(), string.end());
+        case Wchar:
+        {
+            // 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 {};
+        case Invalid:
+            // TODO(yuriks): Add assert
+            LOG_ERROR(Service_FS, "LowPathType cannot be converted to binary!");
+            return {};
+        }
+    }
+
+private:
+    LowPathType type;
+    std::vector<u8> binary;
+    std::string string;
+    std::u16string u16str;
+};
+
+class ArchiveBackend : NonCopyable {
+public:
+    virtual ~ArchiveBackend() {
+    }
+
+    /**
+     * Get a descriptive name for the archive (e.g. "RomFS", "SaveData", etc.)
+     */
+    virtual std::string GetName() const = 0;
+
+    /**
+     * Open a file specified by its path, using the specified mode
+     * @param path Path relative to the archive
+     * @param mode Mode to open the file with
+     * @return Opened file, or nullptr
+     */
+    virtual std::unique_ptr<FileBackend> OpenFile(const Path& path, const Mode mode) const = 0;
+
+    /**
+     * Delete a file specified by its path
+     * @param path Path relative to the archive
+     * @return Whether the file could be deleted
+     */
+    virtual bool DeleteFile(const Path& path) const = 0;
+
+    /**
+     * Rename a File specified by its path
+     * @param src_path Source path relative to the archive
+     * @param dest_path Destination path relative to the archive
+     * @return Whether rename succeeded
+     */
+    virtual bool RenameFile(const Path& src_path, const Path& dest_path) const = 0;
+
+    /**
+     * Delete a directory specified by its path
+     * @param path Path relative to the archive
+     * @return Whether the directory could be deleted
+     */
+    virtual bool DeleteDirectory(const Path& path) const = 0;
+
+    /**
+     * Create a file specified by its path
+     * @param path Path relative to the Archive
+     * @param size The size of the new file, filled with zeroes
+     * @return File creation result code
+     */
+    virtual ResultCode CreateFile(const Path& path, u32 size) const = 0;
+
+    /**
+     * Create a directory specified by its path
+     * @param path Path relative to the archive
+     * @return Whether the directory could be created
+     */
+    virtual bool CreateDirectory(const Path& path) const = 0;
+
+    /**
+     * Rename a Directory specified by its path
+     * @param src_path Source path relative to the archive
+     * @param dest_path Destination path relative to the archive
+     * @return Whether rename succeeded
+     */
+    virtual bool RenameDirectory(const Path& src_path, const Path& dest_path) const = 0;
+
+    /**
+     * Open a directory specified by its path
+     * @param path Path relative to the archive
+     * @return Opened directory, or nullptr
+     */
+    virtual std::unique_ptr<DirectoryBackend> OpenDirectory(const Path& path) const = 0;
+};
+
+} // namespace FileSys
diff --git a/src/core/file_sys/archive_romfs.cpp b/src/core/file_sys/archive_romfs.cpp
index 74974c2df..fdaf73179 100644
--- a/src/core/file_sys/archive_romfs.cpp
+++ b/src/core/file_sys/archive_romfs.cpp
@@ -1,8 +1,11 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
+#include <memory>
+
 #include "common/common_types.h"
+#include "common/make_unique.h"
 
 #include "core/file_sys/archive_romfs.h"
 #include "core/file_sys/directory_romfs.h"
@@ -20,107 +23,43 @@ Archive_RomFS::Archive_RomFS(const Loader::AppLoader& app_loader) {
     }
 }
 
-Archive_RomFS::~Archive_RomFS() {
-}
-
-/**
- * Open a file specified by its path, using the specified mode
- * @param path Path relative to the archive
- * @param mode Mode to open the file with
- * @return Opened file, or nullptr
- */
-std::unique_ptr<File> Archive_RomFS::OpenFile(const Path& path, const Mode mode) const {
-    return std::unique_ptr<File>(new File_RomFS);
+std::unique_ptr<FileBackend> Archive_RomFS::OpenFile(const Path& path, const Mode mode) const {
+    return Common::make_unique<File_RomFS>(this);
 }
 
-/**
- * Delete a file specified by its path
- * @param path Path relative to the archive
- * @return Whether the file could be deleted
- */
-bool Archive_RomFS::DeleteFile(const FileSys::Path& path) const {
+bool Archive_RomFS::DeleteFile(const Path& path) const {
     LOG_WARNING(Service_FS, "Attempted to delete a file from ROMFS.");
     return false;
 }
 
-bool Archive_RomFS::RenameFile(const FileSys::Path& src_path, const FileSys::Path& dest_path) const {
+bool Archive_RomFS::RenameFile(const Path& src_path, const Path& dest_path) const {
     LOG_WARNING(Service_FS, "Attempted to rename a file within ROMFS.");
     return false;
 }
 
-/**
- * Delete a directory specified by its path
- * @param path Path relative to the archive
- * @return Whether the directory could be deleted
- */
-bool Archive_RomFS::DeleteDirectory(const FileSys::Path& path) const {
+bool Archive_RomFS::DeleteDirectory(const Path& path) const {
     LOG_WARNING(Service_FS, "Attempted to delete a directory from ROMFS.");
     return false;
 }
 
-/**
- * Create a directory specified by its path
- * @param path Path relative to the archive
- * @return Whether the directory could be created
- */
+ResultCode Archive_RomFS::CreateFile(const Path& path, u32 size) const {
+    LOG_WARNING(Service_FS, "Attempted to create a file in ROMFS.");
+    // TODO: Verify error code
+    return ResultCode(ErrorDescription::NotAuthorized, ErrorModule::FS, ErrorSummary::NotSupported, ErrorLevel::Permanent);
+}
+
 bool Archive_RomFS::CreateDirectory(const Path& path) const {
     LOG_WARNING(Service_FS, "Attempted to create a directory in ROMFS.");
     return false;
 }
 
-bool Archive_RomFS::RenameDirectory(const FileSys::Path& src_path, const FileSys::Path& dest_path) const {
+bool Archive_RomFS::RenameDirectory(const Path& src_path, const Path& dest_path) const {
     LOG_WARNING(Service_FS, "Attempted to rename a file within ROMFS.");
     return false;
 }
 
-/**
- * Open a directory specified by its path
- * @param path Path relative to the archive
- * @return Opened directory, or nullptr
- */
-std::unique_ptr<Directory> Archive_RomFS::OpenDirectory(const Path& path) const {
-    return std::unique_ptr<Directory>(new Directory_RomFS);
-}
-
-/**
- * Read data from the archive
- * @param offset Offset in bytes to start reading data from
- * @param length Length in bytes of data to read from archive
- * @param buffer Buffer to read data into
- * @return Number of bytes read
- */
-size_t Archive_RomFS::Read(const u64 offset, const u32 length, u8* buffer) const {
-    LOG_TRACE(Service_FS, "called offset=%llu, length=%d", offset, length);
-    memcpy(buffer, &raw_data[(u32)offset], length);
-    return length;
-}
-
-/**
- * Write data to the archive
- * @param offset Offset in bytes to start writing data to
- * @param length Length in bytes of data to write to archive
- * @param buffer Buffer to write data from
- * @param flush  The flush parameters (0 == do not flush)
- * @return Number of bytes written
- */
-size_t Archive_RomFS::Write(const u64 offset, const u32 length, const u32 flush, u8* buffer) {
-    LOG_WARNING(Service_FS, "Attempted to write to ROMFS.");
-    return 0;
-}
-
-/**
- * Get the size of the archive in bytes
- * @return Size of the archive in bytes
- */
-size_t Archive_RomFS::GetSize() const {
-    return sizeof(u8) * raw_data.size();
-}
-
-/**
- * Set the size of the archive in bytes
- */
-void Archive_RomFS::SetSize(const u64 size) {
-    LOG_WARNING(Service_FS, "Attempted to set the size of ROMFS");
+std::unique_ptr<DirectoryBackend> Archive_RomFS::OpenDirectory(const Path& path) const {
+    return Common::make_unique<Directory_RomFS>();
 }
 
 } // namespace FileSys
diff --git a/src/core/file_sys/archive_romfs.h b/src/core/file_sys/archive_romfs.h
index 222bdc356..5e918f92d 100644
--- a/src/core/file_sys/archive_romfs.h
+++ b/src/core/file_sys/archive_romfs.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -8,7 +8,7 @@
 
 #include "common/common_types.h"
 
-#include "core/file_sys/archive.h"
+#include "core/file_sys/archive_backend.h"
 #include "core/loader/loader.h"
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -17,16 +17,11 @@
 namespace FileSys {
 
 /// File system interface to the RomFS archive
-class Archive_RomFS final : public Archive {
+class Archive_RomFS final : public ArchiveBackend {
 public:
     Archive_RomFS(const Loader::AppLoader& app_loader);
-    ~Archive_RomFS() override;
 
-    /**
-     * Get the IdCode of the archive (e.g. RomFS, SaveData, etc.)
-     * @return IdCode of the archive
-     */
-    IdCode GetIdCode() const override { return IdCode::RomFS; }
+    std::string GetName() const override { return "RomFS"; }
 
     /**
      * Open a file specified by its path, using the specified mode
@@ -34,14 +29,14 @@ public:
      * @param mode Mode to open the file with
      * @return Opened file, or nullptr
      */
-    std::unique_ptr<File> OpenFile(const Path& path, const Mode mode) const override;
+    std::unique_ptr<FileBackend> OpenFile(const Path& path, const Mode mode) const override;
 
     /**
      * Delete a file specified by its path
      * @param path Path relative to the archive
      * @return Whether the file could be deleted
      */
-    bool DeleteFile(const FileSys::Path& path) const override;
+    bool DeleteFile(const Path& path) const override;
 
     /**
      * Rename a File specified by its path
@@ -49,14 +44,22 @@ public:
      * @param dest_path Destination path relative to the archive
      * @return Whether rename succeeded
      */
-    bool RenameFile(const FileSys::Path& src_path, const FileSys::Path& dest_path) const override;
+    bool RenameFile(const Path& src_path, const Path& dest_path) const override;
 
     /**
      * Delete a directory specified by its path
      * @param path Path relative to the archive
      * @return Whether the directory could be deleted
      */
-    bool DeleteDirectory(const FileSys::Path& path) const override;
+    bool DeleteDirectory(const Path& path) const override;
+
+    /**
+     * Create a file specified by its path
+     * @param path Path relative to the Archive
+     * @param size The size of the new file, filled with zeroes
+     * @return File creation result code
+     */
+    ResultCode CreateFile(const Path& path, u32 size) const override;
 
     /**
      * Create a directory specified by its path
@@ -71,46 +74,18 @@ public:
      * @param dest_path Destination path relative to the archive
      * @return Whether rename succeeded
      */
-    bool RenameDirectory(const FileSys::Path& src_path, const FileSys::Path& dest_path) const override;
+    bool RenameDirectory(const Path& src_path, const Path& dest_path) const override;
 
     /**
      * Open a directory specified by its path
      * @param path Path relative to the archive
      * @return Opened directory, or nullptr
      */
-    std::unique_ptr<Directory> OpenDirectory(const Path& path) const override;
-
-    /**
-     * Read data from the archive
-     * @param offset Offset in bytes to start reading data from
-     * @param length Length in bytes of data to read from archive
-     * @param buffer Buffer to read data into
-     * @return Number of bytes read
-     */
-    size_t Read(const u64 offset, const u32 length, u8* buffer) const override;
-
-    /**
-     * Write data to the archive
-     * @param offset Offset in bytes to start writing data to
-     * @param length Length in bytes of data to write to archive
-     * @param buffer Buffer to write data from
-     * @param flush  The flush parameters (0 == do not flush)
-     * @return Number of bytes written
-     */
-    size_t Write(const u64 offset, const u32 length, const u32 flush, u8* buffer) override;
-
-    /**
-     * Get the size of the archive in bytes
-     * @return Size of the archive in bytes
-     */
-    size_t GetSize() const override;
-
-    /**
-     * Set the size of the archive in bytes
-     */
-    void SetSize(const u64 size) override;
+    std::unique_ptr<DirectoryBackend> OpenDirectory(const Path& path) const override;
 
 private:
+    friend class File_RomFS;
+
     std::vector<u8> raw_data;
 };
 
diff --git a/src/core/file_sys/archive_savedata.cpp b/src/core/file_sys/archive_savedata.cpp
new file mode 100644
index 000000000..97853567c
--- /dev/null
+++ b/src/core/file_sys/archive_savedata.cpp
@@ -0,0 +1,33 @@
+// Copyright 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#include <sys/stat.h>
+
+#include "common/common_types.h"
+#include "common/file_util.h"
+
+#include "core/file_sys/archive_savedata.h"
+#include "core/file_sys/disk_archive.h"
+#include "core/settings.h"
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// FileSys namespace
+
+namespace FileSys {
+
+Archive_SaveData::Archive_SaveData(const std::string& mount_point, u64 program_id)
+        : DiskArchive(mount_point + Common::StringFromFormat("%016X", program_id) + DIR_SEP) {
+    LOG_INFO(Service_FS, "Directory %s set as SaveData.", this->mount_point.c_str());
+}
+
+bool Archive_SaveData::Initialize() {
+    if (!FileUtil::CreateFullPath(mount_point)) {
+        LOG_ERROR(Service_FS, "Unable to create SaveData path.");
+        return false;
+    }
+
+    return true;
+}
+
+} // namespace FileSys
diff --git a/src/core/file_sys/archive_savedata.h b/src/core/file_sys/archive_savedata.h
new file mode 100644
index 000000000..5b0ce29e6
--- /dev/null
+++ b/src/core/file_sys/archive_savedata.h
@@ -0,0 +1,31 @@
+// Copyright 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#pragma once
+
+#include "common/common_types.h"
+
+#include "core/file_sys/disk_archive.h"
+#include "core/loader/loader.h"
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// FileSys namespace
+
+namespace FileSys {
+
+/// File system interface to the SaveData archive
+class Archive_SaveData final : public DiskArchive {
+public:
+    Archive_SaveData(const std::string& mount_point, u64 program_id);
+
+    /**
+     * Initialize the archive.
+     * @return true if it initialized successfully
+     */
+    bool Initialize();
+
+    std::string GetName() const override { return "SaveData"; }
+};
+
+} // namespace FileSys
diff --git a/src/core/file_sys/archive_sdmc.cpp b/src/core/file_sys/archive_sdmc.cpp
index 9e524b60e..1c1c170b6 100644
--- a/src/core/file_sys/archive_sdmc.cpp
+++ b/src/core/file_sys/archive_sdmc.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include <sys/stat.h>
@@ -8,8 +8,7 @@
 #include "common/file_util.h"
 
 #include "core/file_sys/archive_sdmc.h"
-#include "core/file_sys/directory_sdmc.h"
-#include "core/file_sys/file_sdmc.h"
+#include "core/file_sys/disk_archive.h"
 #include "core/settings.h"
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -17,18 +16,10 @@
 
 namespace FileSys {
 
-Archive_SDMC::Archive_SDMC(const std::string& mount_point) {
-    this->mount_point = mount_point;
+Archive_SDMC::Archive_SDMC(const std::string& mount_point) : DiskArchive(mount_point) {
     LOG_INFO(Service_FS, "Directory %s set as SDMC.", mount_point.c_str());
 }
 
-Archive_SDMC::~Archive_SDMC() {
-}
-
-/**
- * Initialize the archive.
- * @return true if it initialized successfully
- */
 bool Archive_SDMC::Initialize() {
     if (!Settings::values.use_virtual_sd) {
         LOG_WARNING(Service_FS, "SDMC disabled by config.");
@@ -43,115 +34,4 @@ bool Archive_SDMC::Initialize() {
     return true;
 }
 
-/**
- * Open a file specified by its path, using the specified mode
- * @param path Path relative to the archive
- * @param mode Mode to open the file with
- * @return Opened file, or nullptr
- */
-std::unique_ptr<File> Archive_SDMC::OpenFile(const Path& path, const Mode mode) const {
-    LOG_DEBUG(Service_FS, "called path=%s mode=%u", path.DebugStr().c_str(), mode.hex);
-    File_SDMC* file = new File_SDMC(this, path, mode);
-    if (!file->Open())
-        return nullptr;
-    return std::unique_ptr<File>(file);
-}
-
-/**
- * Delete a file specified by its path
- * @param path Path relative to the archive
- * @return Whether the file could be deleted
- */
-bool Archive_SDMC::DeleteFile(const FileSys::Path& path) const {
-    return FileUtil::Delete(GetMountPoint() + path.AsString());
-}
-
-bool Archive_SDMC::RenameFile(const FileSys::Path& src_path, const FileSys::Path& dest_path) const {
-    return FileUtil::Rename(GetMountPoint() + src_path.AsString(), GetMountPoint() + dest_path.AsString());
-}
-
-/**
- * Delete a directory specified by its path
- * @param path Path relative to the archive
- * @return Whether the directory could be deleted
- */
-bool Archive_SDMC::DeleteDirectory(const FileSys::Path& path) const {
-    return FileUtil::DeleteDir(GetMountPoint() + path.AsString());
-}
-
-/**
- * Create a directory specified by its path
- * @param path Path relative to the archive
- * @return Whether the directory could be created
- */
-bool Archive_SDMC::CreateDirectory(const Path& path) const {
-    return FileUtil::CreateDir(GetMountPoint() + path.AsString());
-}
-
-bool Archive_SDMC::RenameDirectory(const FileSys::Path& src_path, const FileSys::Path& dest_path) const {
-    return FileUtil::Rename(GetMountPoint() + src_path.AsString(), GetMountPoint() + dest_path.AsString());
-}
-
-/**
- * Open a directory specified by its path
- * @param path Path relative to the archive
- * @return Opened directory, or nullptr
- */
-std::unique_ptr<Directory> Archive_SDMC::OpenDirectory(const Path& path) const {
-    LOG_DEBUG(Service_FS, "called path=%s", path.DebugStr().c_str());
-    Directory_SDMC* directory = new Directory_SDMC(this, path);
-    if (!directory->Open())
-        return nullptr;
-    return std::unique_ptr<Directory>(directory);
-}
-
-/**
- * Read data from the archive
- * @param offset Offset in bytes to start reading archive from
- * @param length Length in bytes to read data from archive
- * @param buffer Buffer to read data into
- * @return Number of bytes read
- */
-size_t Archive_SDMC::Read(const u64 offset, const u32 length, u8* buffer) const {
-    LOG_ERROR(Service_FS, "(UNIMPLEMENTED)");
-    return -1;
-}
-
-/**
- * Write data to the archive
- * @param offset Offset in bytes to start writing data to
- * @param length Length in bytes of data to write to archive
- * @param buffer Buffer to write data from
- * @param flush  The flush parameters (0 == do not flush)
- * @return Number of bytes written
- */
-size_t Archive_SDMC::Write(const u64 offset, const u32 length, const u32 flush, u8* buffer) {
-    LOG_ERROR(Service_FS, "(UNIMPLEMENTED)");
-    return -1;
-}
-
-/**
- * Get the size of the archive in bytes
- * @return Size of the archive in bytes
- */
-size_t Archive_SDMC::GetSize() const {
-    LOG_ERROR(Service_FS, "(UNIMPLEMENTED)");
-    return 0;
-}
-
-/**
- * Set the size of the archive in bytes
- */
-void Archive_SDMC::SetSize(const u64 size) {
-    LOG_ERROR(Service_FS, "(UNIMPLEMENTED)");
-}
-
-/**
- * Getter for the path used for this Archive
- * @return Mount point of that passthrough archive
- */
-std::string Archive_SDMC::GetMountPoint() const {
-    return mount_point;
-}
-
 } // namespace FileSys
diff --git a/src/core/file_sys/archive_sdmc.h b/src/core/file_sys/archive_sdmc.h
index 19f563a62..1b801f217 100644
--- a/src/core/file_sys/archive_sdmc.h
+++ b/src/core/file_sys/archive_sdmc.h
@@ -1,12 +1,12 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
 
 #include "common/common_types.h"
 
-#include "core/file_sys/archive.h"
+#include "core/file_sys/disk_archive.h"
 #include "core/loader/loader.h"
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -15,10 +15,9 @@
 namespace FileSys {
 
 /// File system interface to the SDMC archive
-class Archive_SDMC final : public Archive {
+class Archive_SDMC final : public DiskArchive {
 public:
     Archive_SDMC(const std::string& mount_point);
-    ~Archive_SDMC() override;
 
     /**
      * Initialize the archive.
@@ -26,102 +25,7 @@ public:
      */
     bool Initialize();
 
-    /**
-     * Get the IdCode of the archive (e.g. RomFS, SaveData, etc.)
-     * @return IdCode of the archive
-     */
-    IdCode GetIdCode() const override { return IdCode::SDMC; }
-
-    /**
-     * Open a file specified by its path, using the specified mode
-     * @param path Path relative to the archive
-     * @param mode Mode to open the file with
-     * @return Opened file, or nullptr
-     */
-    std::unique_ptr<File> OpenFile(const Path& path, const Mode mode) const override;
-
-    /**
-     * Delete a file specified by its path
-     * @param path Path relative to the archive
-     * @return Whether the file could be deleted
-     */
-    bool DeleteFile(const FileSys::Path& path) const override;
-
-    /**
-     * Rename a File specified by its path
-     * @param src_path Source path relative to the archive
-     * @param dest_path Destination path relative to the archive
-     * @return Whether rename succeeded
-     */
-    bool RenameFile(const FileSys::Path& src_path, const FileSys::Path& dest_path) const override;
-
-    /**
-     * Delete a directory specified by its path
-     * @param path Path relative to the archive
-     * @return Whether the directory could be deleted
-     */
-    bool DeleteDirectory(const FileSys::Path& path) const override;
-
-    /**
-     * Create a directory specified by its path
-     * @param path Path relative to the archive
-     * @return Whether the directory could be created
-     */
-    bool CreateDirectory(const Path& path) const override;
-
-    /**
-     * Rename a Directory specified by its path
-     * @param src_path Source path relative to the archive
-     * @param dest_path Destination path relative to the archive
-     * @return Whether rename succeeded
-     */
-    bool RenameDirectory(const FileSys::Path& src_path, const FileSys::Path& dest_path) const override;
-
-    /**
-     * Open a directory specified by its path
-     * @param path Path relative to the archive
-     * @return Opened directory, or nullptr
-     */
-    std::unique_ptr<Directory> OpenDirectory(const Path& path) const override;
-
-    /**
-     * Read data from the archive
-     * @param offset Offset in bytes to start reading archive from
-     * @param length Length in bytes to read data from archive
-     * @param buffer Buffer to read data into
-     * @return Number of bytes read
-     */
-    size_t Read(const u64 offset, const u32 length, u8* buffer) const override;
-
-    /**
-     * Write data to the archive
-     * @param offset Offset in bytes to start writing data to
-     * @param length Length in bytes of data to write to archive
-     * @param buffer Buffer to write data from
-     * @param flush  The flush parameters (0 == do not flush)
-     * @return Number of bytes written
-     */
-    size_t Write(const u64 offset, const u32 length, const u32 flush, u8* buffer) override;
-
-    /**
-     * Get the size of the archive in bytes
-     * @return Size of the archive in bytes
-     */
-    size_t GetSize() const override;
-
-    /**
-     * Set the size of the archive in bytes
-     */
-    void SetSize(const u64 size) override;
-
-    /**
-     * Getter for the path used for this Archive
-     * @return Mount point of that passthrough archive
-     */
-    std::string GetMountPoint() const;
-
-private:
-    std::string mount_point;
+    std::string GetName() const override { return "SDMC"; }
 };
 
 } // namespace FileSys
diff --git a/src/core/file_sys/archive_systemsavedata.cpp b/src/core/file_sys/archive_systemsavedata.cpp
new file mode 100644
index 000000000..0da32d510
--- /dev/null
+++ b/src/core/file_sys/archive_systemsavedata.cpp
@@ -0,0 +1,39 @@
+// Copyright 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#include <sys/stat.h>
+
+#include "common/common_types.h"
+#include "common/file_util.h"
+
+#include "core/file_sys/archive_systemsavedata.h"
+#include "core/file_sys/disk_archive.h"
+#include "core/settings.h"
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// FileSys namespace
+
+namespace FileSys {
+
+static std::string GetSystemSaveDataPath(const std::string& mount_point, u64 save_id) {
+    u32 save_high = static_cast<u32>((save_id >> 32) & 0xFFFFFFFF);
+    u32 save_low = static_cast<u32>(save_id & 0xFFFFFFFF);
+    return Common::StringFromFormat("%s%08X/%08X/", mount_point.c_str(), save_low, save_high);
+}
+
+Archive_SystemSaveData::Archive_SystemSaveData(const std::string& mount_point, u64 save_id)
+        : DiskArchive(GetSystemSaveDataPath(mount_point, save_id)) {
+    LOG_INFO(Service_FS, "Directory %s set as SystemSaveData.", this->mount_point.c_str());
+}
+
+bool Archive_SystemSaveData::Initialize() {
+    if (!FileUtil::CreateFullPath(mount_point)) {
+        LOG_ERROR(Service_FS, "Unable to create SystemSaveData path.");
+        return false;
+    }
+
+    return true;
+}
+
+} // namespace FileSys
diff --git a/src/core/file_sys/archive_systemsavedata.h b/src/core/file_sys/archive_systemsavedata.h
new file mode 100644
index 000000000..55d85193c
--- /dev/null
+++ b/src/core/file_sys/archive_systemsavedata.h
@@ -0,0 +1,33 @@
+// Copyright 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#pragma once
+
+#include "common/common_types.h"
+
+#include "core/file_sys/disk_archive.h"
+#include "core/loader/loader.h"
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// FileSys namespace
+
+namespace FileSys {
+
+/// File system interface to the SystemSaveData archive
+/// TODO(Subv): This archive should point to a location in the NAND,
+/// specifically nand:/data/<ID0>/sysdata/<SaveID-Low>/<SaveID-High>
+class Archive_SystemSaveData final : public DiskArchive {
+public:
+    Archive_SystemSaveData(const std::string& mount_point, u64 save_id);
+
+    /**
+     * Initialize the archive.
+     * @return true if it initialized successfully
+     */
+    bool Initialize();
+
+    std::string GetName() const override { return "SystemSaveData"; }
+};
+
+} // namespace FileSys
diff --git a/src/core/file_sys/directory.h b/src/core/file_sys/directory_backend.h
index 1bb4101d6..7f327dc42 100644
--- a/src/core/file_sys/directory.h
+++ b/src/core/file_sys/directory_backend.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -36,10 +36,10 @@ static_assert(offsetof(Entry, extension) == 0x216, "Wrong offset for extension i
 static_assert(offsetof(Entry, is_archive) == 0x21E, "Wrong offset for is_archive in Entry.");
 static_assert(offsetof(Entry, file_size) == 0x220, "Wrong offset for file_size in Entry.");
 
-class Directory : NonCopyable {
+class DirectoryBackend : NonCopyable {
 public:
-    Directory() { }
-    virtual ~Directory() { }
+    DirectoryBackend() { }
+    virtual ~DirectoryBackend() { }
 
     /**
     * Open the directory
diff --git a/src/core/file_sys/directory_romfs.cpp b/src/core/file_sys/directory_romfs.cpp
index e6d571391..e130aca17 100644
--- a/src/core/file_sys/directory_romfs.cpp
+++ b/src/core/file_sys/directory_romfs.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "common/common_types.h"
@@ -21,20 +21,10 @@ bool Directory_RomFS::Open() {
     return false;
 }
 
-/**
- * List files contained in the directory
- * @param count Number of entries to return at once in entries
- * @param entries Buffer to read data into
- * @return Number of entries listed
- */
 u32 Directory_RomFS::Read(const u32 count, Entry* entries) {
     return 0;
 }
 
-/**
- * Close the directory
- * @return true if the directory closed correctly
- */
 bool Directory_RomFS::Close() const {
     return false;
 }
diff --git a/src/core/file_sys/directory_romfs.h b/src/core/file_sys/directory_romfs.h
index e2944099e..2297f1645 100644
--- a/src/core/file_sys/directory_romfs.h
+++ b/src/core/file_sys/directory_romfs.h
@@ -1,12 +1,12 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
 
 #include "common/common_types.h"
 
-#include "core/file_sys/directory.h"
+#include "core/file_sys/directory_backend.h"
 #include "core/loader/loader.h"
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -14,7 +14,7 @@
 
 namespace FileSys {
 
-class Directory_RomFS final : public Directory {
+class Directory_RomFS final : public DirectoryBackend {
 public:
     Directory_RomFS();
     ~Directory_RomFS() override;
diff --git a/src/core/file_sys/directory_sdmc.cpp b/src/core/file_sys/directory_sdmc.cpp
deleted file mode 100644
index 519787641..000000000
--- a/src/core/file_sys/directory_sdmc.cpp
+++ /dev/null
@@ -1,88 +0,0 @@
-// Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
-// Refer to the license.txt file included.
-
-#include <sys/stat.h>
-
-#include "common/common_types.h"
-#include "common/file_util.h"
-
-#include "core/file_sys/directory_sdmc.h"
-#include "core/file_sys/archive_sdmc.h"
-
-////////////////////////////////////////////////////////////////////////////////////////////////////
-// FileSys namespace
-
-namespace FileSys {
-
-Directory_SDMC::Directory_SDMC(const Archive_SDMC* archive, const Path& path) {
-    // TODO(Link Mauve): normalize path into an absolute path without "..", it can currently bypass
-    // the root directory we set while opening the archive.
-    // For example, opening /../../usr/bin can give the emulated program your installed programs.
-    this->path = archive->GetMountPoint() + path.AsString();
-
-}
-
-Directory_SDMC::~Directory_SDMC() {
-    Close();
-}
-
-bool Directory_SDMC::Open() {
-    if (!FileUtil::IsDirectory(path))
-        return false;
-    FileUtil::ScanDirectoryTree(path, directory);
-    children_iterator = directory.children.begin();
-    return true;
-}
-
-/**
- * List files contained in the directory
- * @param count Number of entries to return at once in entries
- * @param entries Buffer to read data into
- * @return Number of entries listed
- */
-u32 Directory_SDMC::Read(const u32 count, Entry* entries) {
-    u32 entries_read = 0;
-
-    while (entries_read < count && children_iterator != directory.children.cend()) {
-        const FileUtil::FSTEntry& file = *children_iterator;
-        const std::string& filename = file.virtualName;
-        Entry& entry = entries[entries_read];
-
-        LOG_TRACE(Service_FS, "File %s: size=%llu dir=%d", filename.c_str(), file.size, file.isDirectory);
-
-        // TODO(Link Mauve): use a proper conversion to UTF-16.
-        for (size_t j = 0; j < FILENAME_LENGTH; ++j) {
-            entry.filename[j] = filename[j];
-            if (!filename[j])
-                break;
-        }
-
-        FileUtil::SplitFilename83(filename, entry.short_name, entry.extension);
-
-        entry.is_directory = file.isDirectory;
-        entry.is_hidden = (filename[0] == '.');
-        entry.is_read_only = 0;
-        entry.file_size = file.size;
-
-        // We emulate a SD card where the archive bit has never been cleared, as it would be on
-        // most user SD cards.
-        // Some homebrews (blargSNES for instance) are known to mistakenly use the archive bit as a
-        // file bit.
-        entry.is_archive = !file.isDirectory;
-
-        ++entries_read;
-        ++children_iterator;
-    }
-    return entries_read;
-}
-
-/**
- * Close the directory
- * @return true if the directory closed correctly
- */
-bool Directory_SDMC::Close() const {
-    return true;
-}
-
-} // namespace FileSys
diff --git a/src/core/file_sys/directory_sdmc.h b/src/core/file_sys/directory_sdmc.h
deleted file mode 100644
index 4c08b0d61..000000000
--- a/src/core/file_sys/directory_sdmc.h
+++ /dev/null
@@ -1,55 +0,0 @@
-// Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
-// Refer to the license.txt file included.
-
-#pragma once
-
-#include "common/common_types.h"
-#include "common/file_util.h"
-
-#include "core/file_sys/directory.h"
-#include "core/file_sys/archive_sdmc.h"
-#include "core/loader/loader.h"
-
-////////////////////////////////////////////////////////////////////////////////////////////////////
-// FileSys namespace
-
-namespace FileSys {
-
-class Directory_SDMC final : public Directory {
-public:
-    Directory_SDMC();
-    Directory_SDMC(const Archive_SDMC* archive, const Path& path);
-    ~Directory_SDMC() override;
-
-    /**
-    * Open the directory
-    * @return true if the directory opened correctly
-    */
-    bool Open() override;
-
-    /**
-     * List files contained in the directory
-     * @param count Number of entries to return at once in entries
-     * @param entries Buffer to read data into
-     * @return Number of entries listed
-     */
-    u32 Read(const u32 count, Entry* entries) override;
-
-    /**
-     * Close the directory
-     * @return true if the directory closed correctly
-     */
-    bool Close() const override;
-
-private:
-    std::string path;
-    u32 total_entries_in_directory;
-    FileUtil::FSTEntry directory;
-
-    // We need to remember the last entry we returned, so a subsequent call to Read will continue
-    // from the next one.  This iterator will always point to the next unread entry.
-    std::vector<FileUtil::FSTEntry>::iterator children_iterator;
-};
-
-} // namespace FileSys
diff --git a/src/core/file_sys/disk_archive.cpp b/src/core/file_sys/disk_archive.cpp
new file mode 100644
index 000000000..0197f727d
--- /dev/null
+++ b/src/core/file_sys/disk_archive.cpp
@@ -0,0 +1,188 @@
+// Copyright 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#include <sys/stat.h>
+
+#include "common/common_types.h"
+#include "common/file_util.h"
+
+#include "core/file_sys/disk_archive.h"
+#include "core/settings.h"
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// FileSys namespace
+
+namespace FileSys {
+
+std::unique_ptr<FileBackend> DiskArchive::OpenFile(const Path& path, const Mode mode) const {
+    LOG_DEBUG(Service_FS, "called path=%s mode=%01X", path.DebugStr().c_str(), mode.hex);
+    DiskFile* file = new DiskFile(this, path, mode);
+    if (!file->Open())
+        return nullptr;
+    return std::unique_ptr<FileBackend>(file);
+}
+
+bool DiskArchive::DeleteFile(const Path& path) const {
+    return FileUtil::Delete(GetMountPoint() + path.AsString());
+}
+
+bool DiskArchive::RenameFile(const Path& src_path, const Path& dest_path) const {
+    return FileUtil::Rename(GetMountPoint() + src_path.AsString(), GetMountPoint() + dest_path.AsString());
+}
+
+bool DiskArchive::DeleteDirectory(const Path& path) const {
+    return FileUtil::DeleteDir(GetMountPoint() + path.AsString());
+}
+
+ResultCode DiskArchive::CreateFile(const FileSys::Path& path, u32 size) const {
+    std::string full_path = GetMountPoint() + path.AsString();
+
+    if (FileUtil::Exists(full_path))
+        return ResultCode(ErrorDescription::AlreadyExists, ErrorModule::FS, ErrorSummary::NothingHappened, ErrorLevel::Info);
+
+    if (size == 0) {
+        FileUtil::CreateEmptyFile(full_path);
+        return RESULT_SUCCESS;
+    }
+
+    FileUtil::IOFile file(full_path, "wb");
+    // Creates a sparse file (or a normal file on filesystems without the concept of sparse files)
+    // We do this by seeking to the right size, then writing a single null byte.
+    if (file.Seek(size - 1, SEEK_SET) && file.WriteBytes("", 1) == 1)
+        return RESULT_SUCCESS;
+
+    return ResultCode(ErrorDescription::TooLarge, ErrorModule::FS, ErrorSummary::OutOfResource, ErrorLevel::Info);
+}
+
+
+bool DiskArchive::CreateDirectory(const Path& path) const {
+    return FileUtil::CreateDir(GetMountPoint() + path.AsString());
+}
+
+bool DiskArchive::RenameDirectory(const Path& src_path, const Path& dest_path) const {
+    return FileUtil::Rename(GetMountPoint() + src_path.AsString(), GetMountPoint() + dest_path.AsString());
+}
+
+std::unique_ptr<DirectoryBackend> DiskArchive::OpenDirectory(const Path& path) const {
+    LOG_DEBUG(Service_FS, "called path=%s", path.DebugStr().c_str());
+    DiskDirectory* directory = new DiskDirectory(this, path);
+    if (!directory->Open())
+        return nullptr;
+    return std::unique_ptr<DirectoryBackend>(directory);
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+
+DiskFile::DiskFile(const DiskArchive* archive, const Path& path, const Mode mode) {
+    // TODO(Link Mauve): normalize path into an absolute path without "..", it can currently bypass
+    // the root directory we set while opening the archive.
+    // For example, opening /../../etc/passwd can give the emulated program your users list.
+    this->path = archive->GetMountPoint() + path.AsString();
+    this->mode.hex = mode.hex;
+    this->archive = archive;
+}
+
+bool DiskFile::Open() {
+    if (!mode.create_flag && !FileUtil::Exists(path)) {
+        LOG_ERROR(Service_FS, "Non-existing file %s can't be open without mode create.", path.c_str());
+        return false;
+    }
+
+    std::string mode_string;
+    if (mode.create_flag)
+        mode_string = "w+";
+    else if (mode.write_flag)
+        mode_string = "r+"; // Files opened with Write access can be read from
+    else if (mode.read_flag)
+        mode_string = "r";
+
+    // Open the file in binary mode, to avoid problems with CR/LF on Windows systems
+    mode_string += "b";
+
+    file = new FileUtil::IOFile(path, mode_string.c_str());
+    return true;
+}
+
+size_t DiskFile::Read(const u64 offset, const u32 length, u8* buffer) const {
+    file->Seek(offset, SEEK_SET);
+    return file->ReadBytes(buffer, length);
+}
+
+size_t DiskFile::Write(const u64 offset, const u32 length, const u32 flush, const u8* buffer) const {
+    file->Seek(offset, SEEK_SET);
+    size_t written = file->WriteBytes(buffer, length);
+    if (flush)
+        file->Flush();
+    return written;
+}
+
+size_t DiskFile::GetSize() const {
+    return static_cast<size_t>(file->GetSize());
+}
+
+bool DiskFile::SetSize(const u64 size) const {
+    file->Resize(size);
+    file->Flush();
+    return true;
+}
+
+bool DiskFile::Close() const {
+    return file->Close();
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+
+DiskDirectory::DiskDirectory(const DiskArchive* archive, const Path& path) {
+    // TODO(Link Mauve): normalize path into an absolute path without "..", it can currently bypass
+    // the root directory we set while opening the archive.
+    // For example, opening /../../usr/bin can give the emulated program your installed programs.
+    this->path = archive->GetMountPoint() + path.AsString();
+    this->archive = archive;
+}
+
+bool DiskDirectory::Open() {
+    if (!FileUtil::IsDirectory(path))
+        return false;
+    FileUtil::ScanDirectoryTree(path, directory);
+    children_iterator = directory.children.begin();
+    return true;
+}
+
+u32 DiskDirectory::Read(const u32 count, Entry* entries) {
+    u32 entries_read = 0;
+
+    while (entries_read < count && children_iterator != directory.children.cend()) {
+        const FileUtil::FSTEntry& file = *children_iterator;
+        const std::string& filename = file.virtualName;
+        Entry& entry = entries[entries_read];
+
+        LOG_TRACE(Service_FS, "File %s: size=%llu dir=%d", filename.c_str(), file.size, file.isDirectory);
+
+        // TODO(Link Mauve): use a proper conversion to UTF-16.
+        for (size_t j = 0; j < FILENAME_LENGTH; ++j) {
+            entry.filename[j] = filename[j];
+            if (!filename[j])
+                break;
+        }
+
+        FileUtil::SplitFilename83(filename, entry.short_name, entry.extension);
+
+        entry.is_directory = file.isDirectory;
+        entry.is_hidden = (filename[0] == '.');
+        entry.is_read_only = 0;
+        entry.file_size = file.size;
+
+        // We emulate a SD card where the archive bit has never been cleared, as it would be on
+        // most user SD cards.
+        // Some homebrews (blargSNES for instance) are known to mistakenly use the archive bit as a
+        // file bit.
+        entry.is_archive = !file.isDirectory;
+
+        ++entries_read;
+        ++children_iterator;
+    }
+    return entries_read;
+}
+
+} // namespace FileSys
diff --git a/src/core/file_sys/disk_archive.h b/src/core/file_sys/disk_archive.h
new file mode 100644
index 000000000..018ebd2ed
--- /dev/null
+++ b/src/core/file_sys/disk_archive.h
@@ -0,0 +1,103 @@
+// Copyright 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#pragma once
+
+#include "common/common_types.h"
+#include "common/file_util.h"
+
+#include "core/file_sys/archive_backend.h"
+#include "core/loader/loader.h"
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// FileSys namespace
+
+namespace FileSys {
+
+/**
+ * Helper which implements a backend accessing the host machine's filesystem.
+ * This should be subclassed by concrete archive types, which will provide the
+ * base directory on the host filesystem and override any required functionality.
+ */
+class DiskArchive : public ArchiveBackend {
+public:
+    DiskArchive(const std::string& mount_point_) : mount_point(mount_point_) {}
+
+    virtual std::string GetName() const = 0;
+    std::unique_ptr<FileBackend> OpenFile(const Path& path, const Mode mode) const override;
+    bool DeleteFile(const Path& path) const override;
+    bool RenameFile(const Path& src_path, const Path& dest_path) const override;
+    bool DeleteDirectory(const Path& path) const override;
+    ResultCode CreateFile(const Path& path, u32 size) const override;
+    bool CreateDirectory(const Path& path) const override;
+    bool RenameDirectory(const Path& src_path, const Path& dest_path) const override;
+    std::unique_ptr<DirectoryBackend> OpenDirectory(const Path& path) const override;
+
+    /**
+     * Getter for the path used for this Archive
+     * @return Mount point of that passthrough archive
+     */
+    const std::string& GetMountPoint() const {
+        return mount_point;
+    }
+
+protected:
+    std::string mount_point;
+};
+
+class DiskFile : public FileBackend {
+public:
+    DiskFile();
+    DiskFile(const DiskArchive* archive, const Path& path, const Mode mode);
+
+    ~DiskFile() override {
+        Close();
+    }
+
+    bool Open() override;
+    size_t Read(const u64 offset, const u32 length, u8* buffer) const override;
+    size_t Write(const u64 offset, const u32 length, const u32 flush, const u8* buffer) const override;
+    size_t GetSize() const override;
+    bool SetSize(const u64 size) const override;
+    bool Close() const override;
+
+    void Flush() const override {
+        file->Flush();
+    }
+
+protected:
+    const DiskArchive* archive;
+    std::string path;
+    Mode mode;
+    FileUtil::IOFile* file;
+};
+
+class DiskDirectory : public DirectoryBackend {
+public:
+    DiskDirectory();
+    DiskDirectory(const DiskArchive* archive, const Path& path);
+
+    ~DiskDirectory() override {
+        Close();
+    }
+
+    bool Open() override;
+    u32 Read(const u32 count, Entry* entries) override;
+
+    bool Close() const override {
+        return true;
+    }
+
+protected:
+    const DiskArchive* archive;
+    std::string path;
+    u32 total_entries_in_directory;
+    FileUtil::FSTEntry directory;
+
+    // We need to remember the last entry we returned, so a subsequent call to Read will continue
+    // from the next one.  This iterator will always point to the next unread entry.
+    std::vector<FileUtil::FSTEntry>::iterator children_iterator;
+};
+
+} // namespace FileSys
diff --git a/src/core/file_sys/file.h b/src/core/file_sys/file_backend.h
index 4013b6c3e..35890af1f 100644
--- a/src/core/file_sys/file.h
+++ b/src/core/file_sys/file_backend.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -13,10 +13,10 @@
 
 namespace FileSys {
 
-class File : NonCopyable {
+class FileBackend : NonCopyable {
 public:
-    File() { }
-    virtual ~File() { }
+    FileBackend() { }
+    virtual ~FileBackend() { }
 
     /**
      * Open the file
@@ -61,6 +61,11 @@ public:
      * @return true if the file closed correctly
      */
     virtual bool Close() const = 0;
+
+    /**
+     * Flushes the file
+     */
+    virtual void Flush() const = 0;
 };
 
 } // namespace FileSys
diff --git a/src/core/file_sys/file_romfs.cpp b/src/core/file_sys/file_romfs.cpp
index b55708df4..7467d6d31 100644
--- a/src/core/file_sys/file_romfs.cpp
+++ b/src/core/file_sys/file_romfs.cpp
@@ -1,74 +1,41 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "common/common_types.h"
 
 #include "core/file_sys/file_romfs.h"
+#include "core/file_sys/archive_romfs.h"
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////
 // FileSys namespace
 
 namespace FileSys {
 
-File_RomFS::File_RomFS() {
-}
-
-File_RomFS::~File_RomFS() {
-}
-
-/**
- * Open the file
- * @return true if the file opened correctly
- */
 bool File_RomFS::Open() {
-    return false;
+    return true;
 }
 
-/**
- * Read data from the file
- * @param offset Offset in bytes to start reading data from
- * @param length Length in bytes of data to read from file
- * @param buffer Buffer to read data into
- * @return Number of bytes read
- */
 size_t File_RomFS::Read(const u64 offset, const u32 length, u8* buffer) const {
-    return -1;
+    LOG_TRACE(Service_FS, "called offset=%llu, length=%d", offset, length);
+    memcpy(buffer, &archive->raw_data[(u32)offset], length);
+    return length;
 }
 
-/**
- * Write data to the file
- * @param offset Offset in bytes to start writing data to
- * @param length Length in bytes of data to write to file
- * @param flush The flush parameters (0 == do not flush)
- * @param buffer Buffer to read data from
- * @return Number of bytes written
- */
 size_t File_RomFS::Write(const u64 offset, const u32 length, const u32 flush, const u8* buffer) const {
-    return -1;
+    LOG_WARNING(Service_FS, "Attempted to write to ROMFS.");
+    return 0;
 }
 
-/**
- * Get the size of the file in bytes
- * @return Size of the file in bytes
- */
 size_t File_RomFS::GetSize() const {
-    return -1;
+    return sizeof(u8) * archive->raw_data.size();
 }
 
-/**
- * Set the size of the file in bytes
- * @param size New size of the file
- * @return true if successful
- */
 bool File_RomFS::SetSize(const u64 size) const {
+    LOG_WARNING(Service_FS, "Attempted to set the size of ROMFS");
     return false;
 }
 
-/**
- * Close the file
- * @return true if the file closed correctly
- */
 bool File_RomFS::Close() const {
     return false;
 }
diff --git a/src/core/file_sys/file_romfs.h b/src/core/file_sys/file_romfs.h
index 5196701d3..04d8a16a2 100644
--- a/src/core/file_sys/file_romfs.h
+++ b/src/core/file_sys/file_romfs.h
@@ -1,12 +1,12 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
 
 #include "common/common_types.h"
 
-#include "core/file_sys/file.h"
+#include "core/file_sys/file_backend.h"
 #include "core/loader/loader.h"
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -14,10 +14,11 @@
 
 namespace FileSys {
 
-class File_RomFS final : public File {
+class Archive_RomFS;
+
+class File_RomFS final : public FileBackend {
 public:
-    File_RomFS();
-    ~File_RomFS() override;
+    File_RomFS(const Archive_RomFS* archive) : archive(archive) {}
 
     /**
      * Open the file
@@ -62,6 +63,11 @@ public:
      * @return true if the file closed correctly
      */
     bool Close() const override;
+
+    void Flush() const override { }
+
+private:
+    const Archive_RomFS* archive;
 };
 
 } // namespace FileSys
diff --git a/src/core/file_sys/file_sdmc.cpp b/src/core/file_sys/file_sdmc.cpp
deleted file mode 100644
index 46c29900b..000000000
--- a/src/core/file_sys/file_sdmc.cpp
+++ /dev/null
@@ -1,110 +0,0 @@
-// Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
-// Refer to the license.txt file included.
-
-#include <sys/stat.h>
-
-#include "common/common_types.h"
-#include "common/file_util.h"
-
-#include "core/file_sys/file_sdmc.h"
-#include "core/file_sys/archive_sdmc.h"
-
-////////////////////////////////////////////////////////////////////////////////////////////////////
-// FileSys namespace
-
-namespace FileSys {
-
-File_SDMC::File_SDMC(const Archive_SDMC* archive, const Path& path, const Mode mode) {
-    // TODO(Link Mauve): normalize path into an absolute path without "..", it can currently bypass
-    // the root directory we set while opening the archive.
-    // For example, opening /../../etc/passwd can give the emulated program your users list.
-    this->path = archive->GetMountPoint() + path.AsString();
-    this->mode.hex = mode.hex;
-}
-
-File_SDMC::~File_SDMC() {
-    Close();
-}
-
-/**
- * Open the file
- * @return true if the file opened correctly
- */
-bool File_SDMC::Open() {
-    if (!mode.create_flag && !FileUtil::Exists(path)) {
-        LOG_ERROR(Service_FS, "Non-existing file %s can’t be open without mode create.", path.c_str());
-        return false;
-    }
-
-    std::string mode_string;
-    if (mode.create_flag)
-        mode_string = "w+";
-    else if (mode.write_flag)
-        mode_string = "r+"; // Files opened with Write access can be read from
-    else if (mode.read_flag)
-        mode_string = "r";
-    
-    // Open the file in binary mode, to avoid problems with CR/LF on Windows systems
-    mode_string += "b";
-
-    file = new FileUtil::IOFile(path, mode_string.c_str());
-    return true;
-}
-
-/**
- * Read data from the file
- * @param offset Offset in bytes to start reading data from
- * @param length Length in bytes of data to read from file
- * @param buffer Buffer to read data into
- * @return Number of bytes read
- */
-size_t File_SDMC::Read(const u64 offset, const u32 length, u8* buffer) const {
-    file->Seek(offset, SEEK_SET);
-    return file->ReadBytes(buffer, length);
-}
-
-/**
- * Write data to the file
- * @param offset Offset in bytes to start writing data to
- * @param length Length in bytes of data to write to file
- * @param flush The flush parameters (0 == do not flush)
- * @param buffer Buffer to read data from
- * @return Number of bytes written
- */
-size_t File_SDMC::Write(const u64 offset, const u32 length, const u32 flush, const u8* buffer) const {
-    file->Seek(offset, SEEK_SET);
-    size_t written = file->WriteBytes(buffer, length);
-    if (flush)
-        file->Flush();
-    return written;
-}
-
-/**
- * Get the size of the file in bytes
- * @return Size of the file in bytes
- */
-size_t File_SDMC::GetSize() const {
-    return static_cast<size_t>(file->GetSize());
-}
-
-/**
- * Set the size of the file in bytes
- * @param size New size of the file
- * @return true if successful
- */
-bool File_SDMC::SetSize(const u64 size) const {
-    file->Resize(size);
-    file->Flush();
-    return true;
-}
-
-/**
- * Close the file
- * @return true if the file closed correctly
- */
-bool File_SDMC::Close() const {
-    return file->Close();
-}
-
-} // namespace FileSys
diff --git a/src/core/file_sys/file_sdmc.h b/src/core/file_sys/file_sdmc.h
deleted file mode 100644
index 80b445968..000000000
--- a/src/core/file_sys/file_sdmc.h
+++ /dev/null
@@ -1,75 +0,0 @@
-// Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
-// Refer to the license.txt file included.
-
-#pragma once
-
-#include "common/common_types.h"
-#include "common/file_util.h"
-
-#include "core/file_sys/file.h"
-#include "core/file_sys/archive_sdmc.h"
-#include "core/loader/loader.h"
-
-////////////////////////////////////////////////////////////////////////////////////////////////////
-// FileSys namespace
-
-namespace FileSys {
-
-class File_SDMC final : public File {
-public:
-    File_SDMC();
-    File_SDMC(const Archive_SDMC* archive, const Path& path, const Mode mode);
-    ~File_SDMC() override;
-
-    /**
-     * Open the file
-     * @return true if the file opened correctly
-     */
-    bool Open() override;
-
-    /**
-     * Read data from the file
-     * @param offset Offset in bytes to start reading data from
-     * @param length Length in bytes of data to read from file
-     * @param buffer Buffer to read data into
-     * @return Number of bytes read
-     */
-    size_t Read(const u64 offset, const u32 length, u8* buffer) const override;
-
-    /**
-     * Write data to the file
-     * @param offset Offset in bytes to start writing data to
-     * @param length Length in bytes of data to write to file
-     * @param flush The flush parameters (0 == do not flush)
-     * @param buffer Buffer to read data from
-     * @return Number of bytes written
-     */
-    size_t Write(const u64 offset, const u32 length, const u32 flush, const u8* buffer) const override;
-
-    /**
-     * Get the size of the file in bytes
-     * @return Size of the file in bytes
-     */
-    size_t GetSize() const override;
-
-    /**
-     * Set the size of the file in bytes
-     * @param size New size of the file
-     * @return true if successful
-     */
-    bool SetSize(const u64 size) const override;
-
-    /**
-     * Close the file
-     * @return true if the file closed correctly
-     */
-    bool Close() const override;
-
-private:
-    std::string path;
-    Mode mode;
-    FileUtil::IOFile* file;
-};
-
-} // namespace FileSys
diff --git a/src/core/hle/config_mem.cpp b/src/core/hle/config_mem.cpp
index d8ba9e6cf..721a600b5 100644
--- a/src/core/hle/config_mem.cpp
+++ b/src/core/hle/config_mem.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "common/common_types.h"
diff --git a/src/core/hle/config_mem.h b/src/core/hle/config_mem.h
index fa01b5cdb..3975af18f 100644
--- a/src/core/hle/config_mem.h
+++ b/src/core/hle/config_mem.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/core/hle/coprocessor.cpp b/src/core/hle/coprocessor.cpp
index e34229a57..425959be4 100644
--- a/src/core/hle/coprocessor.cpp
+++ b/src/core/hle/coprocessor.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "core/hle/coprocessor.h"
diff --git a/src/core/hle/function_wrappers.h b/src/core/hle/function_wrappers.h
index 3dbe25037..3259ce9eb 100644
--- a/src/core/hle/function_wrappers.h
+++ b/src/core/hle/function_wrappers.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -114,6 +114,20 @@ template<s32 func(u32*, const char*)> void Wrap() {
     FuncReturn(retval);
 }
 
+template<s32 func(u32*, s32, s32)> void Wrap() {
+    u32 param_1 = 0;
+    u32 retval = func(&param_1, PARAM(1), PARAM(2));
+    Core::g_app_core->SetReg(1, param_1);
+    FuncReturn(retval);
+}
+
+template<s32 func(s32*, u32, s32)> void Wrap() {
+    s32 param_1 = 0;
+    u32 retval = func(&param_1, PARAM(1), PARAM(2));
+    Core::g_app_core->SetReg(1, param_1);
+    FuncReturn(retval);
+}
+
 ////////////////////////////////////////////////////////////////////////////////////////////////////
 // Function wrappers that return type u32
 
diff --git a/src/core/hle/hle.cpp b/src/core/hle/hle.cpp
index 3f73b5538..33ac12507 100644
--- a/src/core/hle/hle.cpp
+++ b/src/core/hle/hle.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include <vector>
@@ -8,6 +8,8 @@
 #include "core/hle/hle.h"
 #include "core/hle/kernel/thread.h"
 #include "core/hle/service/service.h"
+#include "core/hle/service/fs/archive.h"
+#include "core/hle/service/cfg/cfg.h"
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////
 
@@ -41,7 +43,15 @@ void CallSVC(u32 opcode) {
 
 void Reschedule(const char *reason) {
     _dbg_assert_msg_(Kernel, reason != 0 && strlen(reason) < 256, "Reschedule: Invalid or too long reason.");
+
+    // TODO(bunnei): It seems that games depend on some CPU execution time elapsing during HLE
+    // routines. This simulates that time by artificially advancing the number of CPU "ticks".
+    // The value was chosen empirically, it seems to work well enough for everything tested, but
+    // is likely not ideal. We should find a more accurate way to simulate timing with HLE.
+    Core::g_app_core->AddTicks(4000);
+
     Core::g_app_core->PrepareReschedule();
+
     g_reschedule = true;
 }
 
@@ -56,6 +66,8 @@ void RegisterAllModules() {
 
 void Init() {
     Service::Init();
+    Service::FS::ArchiveInit();
+    Service::CFG::CFGInit();
 
     RegisterAllModules();
 
@@ -63,6 +75,8 @@ void Init() {
 }
 
 void Shutdown() {
+    Service::CFG::CFGShutdown();
+    Service::FS::ArchiveShutdown();
     Service::Shutdown();
 
     g_module_db.clear();
diff --git a/src/core/hle/hle.h b/src/core/hle/hle.h
index 4ab258c69..59b770f02 100644
--- a/src/core/hle/hle.h
+++ b/src/core/hle/hle.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/core/hle/kernel/address_arbiter.cpp b/src/core/hle/kernel/address_arbiter.cpp
index 9a921108d..38705e3cd 100644
--- a/src/core/hle/kernel/address_arbiter.cpp
+++ b/src/core/hle/kernel/address_arbiter.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "common/common_types.h"
@@ -20,8 +20,8 @@ public:
     std::string GetTypeName() const override { return "Arbiter"; }
     std::string GetName() const override { return name; }
 
-    static Kernel::HandleType GetStaticHandleType() { return HandleType::AddressArbiter; }
-    Kernel::HandleType GetHandleType() const override { return HandleType::AddressArbiter; }
+    static const HandleType HANDLE_TYPE = HandleType::AddressArbiter;
+    HandleType GetHandleType() const override { return HANDLE_TYPE; }
 
     std::string name;   ///< Name of address arbiter object (optional)
 };
@@ -62,7 +62,8 @@ ResultCode ArbitrateAddress(Handle handle, ArbitrationType type, u32 address, s3
 /// Create an address arbiter
 AddressArbiter* CreateAddressArbiter(Handle& handle, const std::string& name) {
     AddressArbiter* address_arbiter = new AddressArbiter;
-    handle = Kernel::g_object_pool.Create(address_arbiter);
+    // TOOD(yuriks): Fix error reporting
+    handle = Kernel::g_handle_table.Create(address_arbiter).ValueOr(INVALID_HANDLE);
     address_arbiter->name = name;
     return address_arbiter;
 }
diff --git a/src/core/hle/kernel/address_arbiter.h b/src/core/hle/kernel/address_arbiter.h
index 8a5fb10b4..030e7ad7b 100644
--- a/src/core/hle/kernel/address_arbiter.h
+++ b/src/core/hle/kernel/address_arbiter.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/core/hle/kernel/archive.cpp b/src/core/hle/kernel/archive.cpp
deleted file mode 100644
index ddc09e13b..000000000
--- a/src/core/hle/kernel/archive.cpp
+++ /dev/null
@@ -1,435 +0,0 @@
-// Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
-// Refer to the license.txt file included.
-
-#include "common/common_types.h"
-#include "common/file_util.h"
-#include "common/math_util.h"
-
-#include "core/file_sys/archive.h"
-#include "core/file_sys/archive_sdmc.h"
-#include "core/file_sys/directory.h"
-#include "core/hle/kernel/archive.h"
-#include "core/hle/result.h"
-#include "core/hle/service/service.h"
-
-////////////////////////////////////////////////////////////////////////////////////////////////////
-// Kernel namespace
-
-namespace Kernel {
-
-// Command to access archive file
-enum class FileCommand : u32 {
-    Dummy1          = 0x000100C6,
-    Control         = 0x040100C4,
-    OpenSubFile     = 0x08010100,
-    Read            = 0x080200C2,
-    Write           = 0x08030102,
-    GetSize         = 0x08040000,
-    SetSize         = 0x08050080,
-    GetAttributes   = 0x08060000,
-    SetAttributes   = 0x08070040,
-    Close           = 0x08080000,
-    Flush           = 0x08090000,
-};
-
-// Command to access directory
-enum class DirectoryCommand : u32 {
-    Dummy1          = 0x000100C6,
-    Control         = 0x040100C4,
-    Read            = 0x08010042,
-    Close           = 0x08020000,
-};
-
-class Archive : public Object {
-public:
-    std::string GetTypeName() const override { return "Archive"; }
-    std::string GetName() const override { return name; }
-
-    static Kernel::HandleType GetStaticHandleType() { return HandleType::Archive; }
-    Kernel::HandleType GetHandleType() const override { return HandleType::Archive; }
-
-    std::string name;           ///< Name of archive (optional)
-    FileSys::Archive* backend;  ///< Archive backend interface
-
-    ResultVal<bool> SyncRequest() override {
-        u32* cmd_buff = Service::GetCommandBuffer();
-        FileCommand cmd = static_cast<FileCommand>(cmd_buff[0]);
-
-        switch (cmd) {
-        // Read from archive...
-        case FileCommand::Read:
-        {
-            u64 offset  = cmd_buff[1] | ((u64)cmd_buff[2] << 32);
-            u32 length  = cmd_buff[3];
-            u32 address = cmd_buff[5];
-
-            // Number of bytes read
-            cmd_buff[2] = backend->Read(offset, length, Memory::GetPointer(address));
-            break;
-        }
-        // Write to archive...
-        case FileCommand::Write:
-        {
-            u64 offset  = cmd_buff[1] | ((u64)cmd_buff[2] << 32);
-            u32 length  = cmd_buff[3];
-            u32 flush   = cmd_buff[4];
-            u32 address = cmd_buff[6];
-
-            // Number of bytes written
-            cmd_buff[2] = backend->Write(offset, length, flush, Memory::GetPointer(address));
-            break;
-        }
-        case FileCommand::GetSize:
-        {
-            u64 filesize = (u64) backend->GetSize();
-            cmd_buff[2]  = (u32) filesize;         // Lower word
-            cmd_buff[3]  = (u32) (filesize >> 32); // Upper word
-            break;
-        }
-        case FileCommand::SetSize:
-        {
-            backend->SetSize(cmd_buff[1] | ((u64)cmd_buff[2] << 32));
-            break;
-        }
-        case FileCommand::Close:
-        {
-            LOG_TRACE(Service_FS, "Close %s %s", GetTypeName().c_str(), GetName().c_str());
-            CloseArchive(backend->GetIdCode());
-            break;
-        }
-        // Unknown command...
-        default:
-        {
-            LOG_ERROR(Service_FS, "Unknown command=0x%08X", cmd);
-            return UnimplementedFunction(ErrorModule::FS);
-        }
-        }
-        cmd_buff[1] = 0; // No error
-        return MakeResult<bool>(false);
-    }
-};
-
-class File : public Object {
-public:
-    std::string GetTypeName() const override { return "File"; }
-    std::string GetName() const override { return path.DebugStr(); }
-
-    static Kernel::HandleType GetStaticHandleType() { return HandleType::File; }
-    Kernel::HandleType GetHandleType() const override { return HandleType::File; }
-
-    FileSys::Path path; ///< Path of the file
-    std::unique_ptr<FileSys::File> backend; ///< File backend interface
-
-    ResultVal<bool> SyncRequest() override {
-        u32* cmd_buff = Service::GetCommandBuffer();
-        FileCommand cmd = static_cast<FileCommand>(cmd_buff[0]);
-        switch (cmd) {
-
-        // Read from file...
-        case FileCommand::Read:
-        {
-            u64 offset = cmd_buff[1] | ((u64) cmd_buff[2]) << 32;
-            u32 length  = cmd_buff[3];
-            u32 address = cmd_buff[5];
-            LOG_TRACE(Service_FS, "Read %s %s: offset=0x%llx length=%d address=0x%x",
-                      GetTypeName().c_str(), GetName().c_str(), offset, length, address);
-            cmd_buff[2] = backend->Read(offset, length, Memory::GetPointer(address));
-            break;
-        }
-
-        // Write to file...
-        case FileCommand::Write:
-        {
-            u64 offset  = cmd_buff[1] | ((u64) cmd_buff[2]) << 32;
-            u32 length  = cmd_buff[3];
-            u32 flush   = cmd_buff[4];
-            u32 address = cmd_buff[6];
-            LOG_TRACE(Service_FS, "Write %s %s: offset=0x%llx length=%d address=0x%x, flush=0x%x",
-                      GetTypeName().c_str(), GetName().c_str(), offset, length, address, flush);
-            cmd_buff[2] = backend->Write(offset, length, flush, Memory::GetPointer(address));
-            break;
-        }
-
-        case FileCommand::GetSize:
-        {
-            LOG_TRACE(Service_FS, "GetSize %s %s", GetTypeName().c_str(), GetName().c_str());
-            u64 size = backend->GetSize();
-            cmd_buff[2] = (u32)size;
-            cmd_buff[3] = size >> 32;
-            break;
-        }
-
-        case FileCommand::SetSize:
-        {
-            u64 size = cmd_buff[1] | ((u64)cmd_buff[2] << 32);
-            LOG_TRACE(Service_FS, "SetSize %s %s size=%llu",
-                    GetTypeName().c_str(), GetName().c_str(), size);
-            backend->SetSize(size);
-            break;
-        }
-
-        case FileCommand::Close:
-        {
-            LOG_TRACE(Service_FS, "Close %s %s", GetTypeName().c_str(), GetName().c_str());
-            Kernel::g_object_pool.Destroy<File>(GetHandle());
-            break;
-        }
-
-        // Unknown command...
-        default:
-            LOG_ERROR(Service_FS, "Unknown command=0x%08X!", cmd);
-            ResultCode error = UnimplementedFunction(ErrorModule::FS);
-            cmd_buff[1] = error.raw; // TODO(Link Mauve): use the correct error code for that.
-            return error;
-        }
-        cmd_buff[1] = 0; // No error
-        return MakeResult<bool>(false);
-    }
-};
-
-class Directory : public Object {
-public:
-    std::string GetTypeName() const override { return "Directory"; }
-    std::string GetName() const override { return path.DebugStr(); }
-
-    static Kernel::HandleType GetStaticHandleType() { return HandleType::Directory; }
-    Kernel::HandleType GetHandleType() const override { return HandleType::Directory; }
-
-    FileSys::Path path; ///< Path of the directory
-    std::unique_ptr<FileSys::Directory> backend; ///< File backend interface
-
-    ResultVal<bool> SyncRequest() override {
-        u32* cmd_buff = Service::GetCommandBuffer();
-        DirectoryCommand cmd = static_cast<DirectoryCommand>(cmd_buff[0]);
-        switch (cmd) {
-
-        // Read from directory...
-        case DirectoryCommand::Read:
-        {
-            u32 count = cmd_buff[1];
-            u32 address = cmd_buff[3];
-            auto entries = reinterpret_cast<FileSys::Entry*>(Memory::GetPointer(address));
-            LOG_TRACE(Service_FS, "Read %s %s: count=%d",
-                    GetTypeName().c_str(), GetName().c_str(), count);
-
-            // Number of entries actually read
-            cmd_buff[2] = backend->Read(count, entries);
-            break;
-        }
-
-        case DirectoryCommand::Close:
-        {
-            LOG_TRACE(Service_FS, "Close %s %s", GetTypeName().c_str(), GetName().c_str());
-            Kernel::g_object_pool.Destroy<Directory>(GetHandle());
-            break;
-        }
-
-        // Unknown command...
-        default:
-            LOG_ERROR(Service_FS, "Unknown command=0x%08X!", cmd);
-            ResultCode error = UnimplementedFunction(ErrorModule::FS);
-            cmd_buff[1] = error.raw; // TODO(Link Mauve): use the correct error code for that.
-            return error;
-        }
-        cmd_buff[1] = 0; // No error
-        return MakeResult<bool>(false);
-    }
-};
-
-////////////////////////////////////////////////////////////////////////////////////////////////////
-
-std::map<FileSys::Archive::IdCode, Handle> g_archive_map; ///< Map of file archives by IdCode
-
-ResultVal<Handle> OpenArchive(FileSys::Archive::IdCode id_code) {
-    auto itr = g_archive_map.find(id_code);
-    if (itr == g_archive_map.end()) {
-        return ResultCode(ErrorDescription::NotFound, ErrorModule::FS,
-                ErrorSummary::NotFound, ErrorLevel::Permanent);
-    }
-
-    return MakeResult<Handle>(itr->second);
-}
-
-ResultCode CloseArchive(FileSys::Archive::IdCode id_code) {
-    auto itr = g_archive_map.find(id_code);
-    if (itr == g_archive_map.end()) {
-        LOG_ERROR(Service_FS, "Cannot close archive %d, does not exist!", (int)id_code);
-        return InvalidHandle(ErrorModule::FS);
-    }
-
-    LOG_TRACE(Service_FS, "Closed archive %d", (int) id_code);
-    return RESULT_SUCCESS;
-}
-
-/**
- * Mounts an archive
- * @param archive Pointer to the archive to mount
- */
-ResultCode MountArchive(Archive* archive) {
-    FileSys::Archive::IdCode id_code = archive->backend->GetIdCode();
-    ResultVal<Handle> archive_handle = OpenArchive(id_code);
-    if (archive_handle.Succeeded()) {
-        LOG_ERROR(Service_FS, "Cannot mount two archives with the same ID code! (%d)", (int) id_code);
-        return archive_handle.Code();
-    }
-    g_archive_map[id_code] = archive->GetHandle();
-    LOG_TRACE(Service_FS, "Mounted archive %s", archive->GetName().c_str());
-    return RESULT_SUCCESS;
-}
-
-ResultCode CreateArchive(FileSys::Archive* backend, const std::string& name) {
-    Archive* archive = new Archive;
-    Handle handle = Kernel::g_object_pool.Create(archive);
-    archive->name = name;
-    archive->backend = backend;
-
-    ResultCode result = MountArchive(archive);
-    if (result.IsError()) {
-        return result;
-    }
-    
-    return RESULT_SUCCESS;
-}
-
-ResultVal<Handle> OpenFileFromArchive(Handle archive_handle, const FileSys::Path& path, const FileSys::Mode mode) {
-    // TODO(bunnei): Binary type files get a raw file pointer to the archive. Currently, we create
-    // the archive file handles at app loading, and then keep them persistent throughout execution.
-    // Archives file handles are just reused and not actually freed until emulation shut down.
-    // Verify if real hardware works this way, or if new handles are created each time
-    if (path.GetType() == FileSys::Binary)
-        // TODO(bunnei): FixMe - this is a hack to compensate for an incorrect FileSys backend
-        // design. While the functionally of this is OK, our implementation decision to separate
-        // normal files from archive file pointers is very likely wrong.
-        // See https://github.com/citra-emu/citra/issues/205
-        return MakeResult<Handle>(archive_handle);
-
-    File* file = new File;
-    Handle handle = Kernel::g_object_pool.Create(file);
-
-    Archive* archive = Kernel::g_object_pool.Get<Archive>(archive_handle);
-    if (archive == nullptr) {
-        return InvalidHandle(ErrorModule::FS);
-    }
-    file->path = path;
-    file->backend = archive->backend->OpenFile(path, mode);
-
-    if (!file->backend) {
-        return ResultCode(ErrorDescription::NotFound, ErrorModule::FS,
-                ErrorSummary::NotFound, ErrorLevel::Permanent);
-    }
-
-    return MakeResult<Handle>(handle);
-}
-
-ResultCode DeleteFileFromArchive(Handle archive_handle, const FileSys::Path& path) {
-    Archive* archive = Kernel::g_object_pool.GetFast<Archive>(archive_handle);
-    if (archive == nullptr)
-        return InvalidHandle(ErrorModule::FS);
-    if (archive->backend->DeleteFile(path))
-        return RESULT_SUCCESS;
-    return ResultCode(ErrorDescription::NoData, ErrorModule::FS, // TODO: verify description
-                      ErrorSummary::Canceled, ErrorLevel::Status);
-}
-
-ResultCode RenameFileBetweenArchives(Handle src_archive_handle, const FileSys::Path& src_path,
-                                     Handle dest_archive_handle, const FileSys::Path& dest_path) {
-    Archive* src_archive = Kernel::g_object_pool.GetFast<Archive>(src_archive_handle);
-    Archive* dest_archive = Kernel::g_object_pool.GetFast<Archive>(dest_archive_handle);
-    if (src_archive == nullptr || dest_archive == nullptr)
-        return InvalidHandle(ErrorModule::FS);
-    if (src_archive == dest_archive) {
-        if (src_archive->backend->RenameFile(src_path, dest_path))
-            return RESULT_SUCCESS;
-    } else {
-        // TODO: Implement renaming across archives
-        return UnimplementedFunction(ErrorModule::FS);
-    }
-    return ResultCode(ErrorDescription::NoData, ErrorModule::FS, // TODO: verify description
-                      ErrorSummary::NothingHappened, ErrorLevel::Status);
-}
-
-ResultCode DeleteDirectoryFromArchive(Handle archive_handle, const FileSys::Path& path) {
-    Archive* archive = Kernel::g_object_pool.GetFast<Archive>(archive_handle);
-    if (archive == nullptr)
-        return InvalidHandle(ErrorModule::FS);
-    if (archive->backend->DeleteDirectory(path))
-        return RESULT_SUCCESS;
-    return ResultCode(ErrorDescription::NoData, ErrorModule::FS, // TODO: verify description
-                      ErrorSummary::Canceled, ErrorLevel::Status);
-}
-
-ResultCode CreateDirectoryFromArchive(Handle archive_handle, const FileSys::Path& path) {
-    Archive* archive = Kernel::g_object_pool.GetFast<Archive>(archive_handle);
-    if (archive == nullptr)
-        return InvalidHandle(ErrorModule::FS);
-    if (archive->backend->CreateDirectory(path))
-        return RESULT_SUCCESS;
-    return ResultCode(ErrorDescription::NoData, ErrorModule::FS, // TODO: verify description
-                      ErrorSummary::Canceled, ErrorLevel::Status);
-}
-
-ResultCode RenameDirectoryBetweenArchives(Handle src_archive_handle, const FileSys::Path& src_path,
-                                          Handle dest_archive_handle, const FileSys::Path& dest_path) {
-    Archive* src_archive = Kernel::g_object_pool.GetFast<Archive>(src_archive_handle);
-    Archive* dest_archive = Kernel::g_object_pool.GetFast<Archive>(dest_archive_handle);
-    if (src_archive == nullptr || dest_archive == nullptr)
-        return InvalidHandle(ErrorModule::FS);
-    if (src_archive == dest_archive) {
-        if (src_archive->backend->RenameDirectory(src_path, dest_path))
-            return RESULT_SUCCESS;
-    } else {
-        // TODO: Implement renaming across archives
-        return UnimplementedFunction(ErrorModule::FS);
-    }
-    return ResultCode(ErrorDescription::NoData, ErrorModule::FS, // TODO: verify description
-                      ErrorSummary::NothingHappened, ErrorLevel::Status);
-}
-
-/**
- * Open a Directory from an Archive
- * @param archive_handle Handle to an open Archive object
- * @param path Path to the Directory inside of the Archive
- * @return Opened Directory object
- */
-ResultVal<Handle> OpenDirectoryFromArchive(Handle archive_handle, const FileSys::Path& path) {
-    Directory* directory = new Directory;
-    Handle handle = Kernel::g_object_pool.Create(directory);
-
-    Archive* archive = Kernel::g_object_pool.Get<Archive>(archive_handle);
-    if (archive == nullptr) {
-        return InvalidHandle(ErrorModule::FS);
-    }
-    directory->path = path;
-    directory->backend = archive->backend->OpenDirectory(path);
-
-    if (!directory->backend) {
-        return ResultCode(ErrorDescription::NotFound, ErrorModule::FS,
-                          ErrorSummary::NotFound, ErrorLevel::Permanent);
-    }
-
-    return MakeResult<Handle>(handle);
-}
-
-/// Initialize archives
-void ArchiveInit() {
-    g_archive_map.clear();
-
-    // TODO(Link Mauve): Add the other archive types (see here for the known types:
-    // http://3dbrew.org/wiki/FS:OpenArchive#Archive_idcodes).  Currently the only half-finished
-    // archive type is SDMC, so it is the only one getting exposed.
-
-    std::string sdmc_directory = FileUtil::GetUserPath(D_SDMC_IDX);
-    auto archive = new FileSys::Archive_SDMC(sdmc_directory);
-    if (archive->Initialize())
-        CreateArchive(archive, "SDMC");
-    else
-        LOG_ERROR(Service_FS, "Can't instantiate SDMC archive with path %s", sdmc_directory.c_str());
-}
-
-/// Shutdown archives
-void ArchiveShutdown() {
-    g_archive_map.clear();
-}
-
-} // namespace Kernel
diff --git a/src/core/hle/kernel/event.cpp b/src/core/hle/kernel/event.cpp
index 288080209..e43c3ee4e 100644
--- a/src/core/hle/kernel/event.cpp
+++ b/src/core/hle/kernel/event.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include <map>
@@ -19,8 +19,8 @@ public:
     std::string GetTypeName() const override { return "Event"; }
     std::string GetName() const override { return name; }
 
-    static Kernel::HandleType GetStaticHandleType() { return Kernel::HandleType::Event; }
-    Kernel::HandleType GetHandleType() const override { return Kernel::HandleType::Event; }
+    static const HandleType HANDLE_TYPE = HandleType::Event;
+    HandleType GetHandleType() const override { return HANDLE_TYPE; }
 
     ResetType intitial_reset_type;          ///< ResetType specified at Event initialization
     ResetType reset_type;                   ///< Current ResetType
@@ -53,7 +53,7 @@ public:
  * @return Result of operation, 0 on success, otherwise error code
  */
 ResultCode SetPermanentLock(Handle handle, const bool permanent_locked) {
-    Event* evt = g_object_pool.Get<Event>(handle);
+    Event* evt = g_handle_table.Get<Event>(handle);
     if (evt == nullptr) return InvalidHandle(ErrorModule::Kernel);
 
     evt->permanent_locked = permanent_locked;
@@ -67,7 +67,7 @@ ResultCode SetPermanentLock(Handle handle, const bool permanent_locked) {
  * @return Result of operation, 0 on success, otherwise error code
  */
 ResultCode SetEventLocked(const Handle handle, const bool locked) {
-    Event* evt = g_object_pool.Get<Event>(handle);
+    Event* evt = g_handle_table.Get<Event>(handle);
     if (evt == nullptr) return InvalidHandle(ErrorModule::Kernel);
 
     if (!evt->permanent_locked) {
@@ -82,7 +82,7 @@ ResultCode SetEventLocked(const Handle handle, const bool locked) {
  * @return Result of operation, 0 on success, otherwise error code
  */
 ResultCode SignalEvent(const Handle handle) {
-    Event* evt = g_object_pool.Get<Event>(handle);
+    Event* evt = g_handle_table.Get<Event>(handle);
     if (evt == nullptr) return InvalidHandle(ErrorModule::Kernel);
 
     // Resume threads waiting for event to signal
@@ -110,7 +110,7 @@ ResultCode SignalEvent(const Handle handle) {
  * @return Result of operation, 0 on success, otherwise error code
  */
 ResultCode ClearEvent(Handle handle) {
-    Event* evt = g_object_pool.Get<Event>(handle);
+    Event* evt = g_handle_table.Get<Event>(handle);
     if (evt == nullptr) return InvalidHandle(ErrorModule::Kernel);
 
     if (!evt->permanent_locked) {
@@ -129,7 +129,8 @@ ResultCode ClearEvent(Handle handle) {
 Event* CreateEvent(Handle& handle, const ResetType reset_type, const std::string& name) {
     Event* evt = new Event;
 
-    handle = Kernel::g_object_pool.Create(evt);
+    // TOOD(yuriks): Fix error reporting
+    handle = Kernel::g_handle_table.Create(evt).ValueOr(INVALID_HANDLE);
 
     evt->locked = true;
     evt->permanent_locked = false;
diff --git a/src/core/hle/kernel/event.h b/src/core/hle/kernel/event.h
index 73aec4e79..da793df1a 100644
--- a/src/core/hle/kernel/event.h
+++ b/src/core/hle/kernel/event.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/core/hle/kernel/kernel.cpp b/src/core/hle/kernel/kernel.cpp
index b38be0a49..e59ed1b57 100644
--- a/src/core/hle/kernel/kernel.cpp
+++ b/src/core/hle/kernel/kernel.cpp
@@ -1,5 +1,5 @@
-// Copyright 2014 Citra Emulator Project / PPSSPP Project
-// Licensed under GPLv2
+// Copyright 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include <algorithm>
@@ -9,95 +9,109 @@
 #include "core/core.h"
 #include "core/hle/kernel/kernel.h"
 #include "core/hle/kernel/thread.h"
-#include "core/hle/kernel/archive.h"
 
 namespace Kernel {
 
 Handle g_main_thread = 0;
-ObjectPool g_object_pool;
+HandleTable g_handle_table;
+u64 g_program_id = 0;
 
-ObjectPool::ObjectPool() {
-    next_id = INITIAL_NEXT_ID;
+HandleTable::HandleTable() {
+    next_generation = 1;
+    Clear();
 }
 
-Handle ObjectPool::Create(Object* obj, int range_bottom, int range_top) {
-    if (range_top > MAX_COUNT) {
-        range_top = MAX_COUNT;
-    }
-    if (next_id >= range_bottom && next_id < range_top) {
-        range_bottom = next_id++;
-    }
-    for (int i = range_bottom; i < range_top; i++) {
-        if (!occupied[i]) {
-            occupied[i] = true;
-            pool[i] = obj;
-            pool[i]->handle = i + HANDLE_OFFSET;
-            return i + HANDLE_OFFSET;
-        }
+ResultVal<Handle> HandleTable::Create(Object* obj) {
+    _dbg_assert_(Kernel, obj != nullptr);
+
+    u16 slot = next_free_slot;
+    if (slot >= generations.size()) {
+        LOG_ERROR(Kernel, "Unable to allocate Handle, too many slots in use.");
+        return ERR_OUT_OF_HANDLES;
     }
-    LOG_ERROR(Kernel, "Unable to allocate kernel object, too many objects slots in use.");
-    return 0;
-}
+    next_free_slot = generations[slot];
 
-bool ObjectPool::IsValid(Handle handle) const {
-    int index = handle - HANDLE_OFFSET;
-    if (index < 0)
-        return false;
-    if (index >= MAX_COUNT)
-        return false;
+    u16 generation = next_generation++;
 
-    return occupied[index];
+    // Overflow count so it fits in the 15 bits dedicated to the generation in the handle.
+    // CTR-OS doesn't use generation 0, so skip straight to 1.
+    if (next_generation >= (1 << 15)) next_generation = 1;
+
+    generations[slot] = generation;
+    intrusive_ptr_add_ref(obj);
+    objects[slot] = obj;
+
+    Handle handle = generation | (slot << 15);
+    obj->handle = handle;
+    return MakeResult<Handle>(handle);
 }
 
-void ObjectPool::Clear() {
-    for (int i = 0; i < MAX_COUNT; i++) {
-        //brutally clear everything, no validation
-        if (occupied[i])
-            delete pool[i];
-        occupied[i] = false;
+ResultVal<Handle> HandleTable::Duplicate(Handle handle) {
+    Object* object = GetGeneric(handle);
+    if (object == nullptr) {
+        LOG_ERROR(Kernel, "Tried to duplicate invalid handle: %08X", handle);
+        return ERR_INVALID_HANDLE;
     }
-    pool.fill(nullptr);
-    next_id = INITIAL_NEXT_ID;
+    return Create(object);
+}
+
+ResultCode HandleTable::Close(Handle handle) {
+    if (!IsValid(handle))
+        return ERR_INVALID_HANDLE;
+
+    size_t slot = GetSlot(handle);
+    u16 generation = GetGeneration(handle);
+
+    intrusive_ptr_release(objects[slot]);
+    objects[slot] = nullptr;
+
+    generations[generation] = next_free_slot;
+    next_free_slot = slot;
+    return RESULT_SUCCESS;
 }
 
-Object* &ObjectPool::operator [](Handle handle)
-{
-    _dbg_assert_msg_(Kernel, IsValid(handle), "GRABBING UNALLOCED KERNEL OBJ");
-    return pool[handle - HANDLE_OFFSET];
+bool HandleTable::IsValid(Handle handle) const {
+    size_t slot = GetSlot(handle);
+    u16 generation = GetGeneration(handle);
+
+    return slot < MAX_COUNT && objects[slot] != nullptr && generations[slot] == generation;
 }
 
-void ObjectPool::List() {
-    for (int i = 0; i < MAX_COUNT; i++) {
-        if (occupied[i]) {
-            if (pool[i]) {
-                LOG_DEBUG(Kernel, "KO %i: %s \"%s\"", i + HANDLE_OFFSET, pool[i]->GetTypeName().c_str(),
-                    pool[i]->GetName().c_str());
-            }
-        }
+Object* HandleTable::GetGeneric(Handle handle) const {
+    if (handle == CurrentThread) {
+        // TODO(yuriks) Directly return the pointer once this is possible.
+        handle = GetCurrentThreadHandle();
+    } else if (handle == CurrentProcess) {
+        LOG_ERROR(Kernel, "Current process (%08X) pseudo-handle not supported", CurrentProcess);
+        return nullptr;
     }
-}
 
-int ObjectPool::GetCount() const {
-    return std::count(occupied.begin(), occupied.end(), true);
+    if (!IsValid(handle)) {
+        return nullptr;
+    }
+    return objects[GetSlot(handle)];
 }
 
-Object* ObjectPool::CreateByIDType(int type) {
-    LOG_ERROR(Kernel, "Unimplemented: %d.", type);
-    return nullptr;
+void HandleTable::Clear() {
+    for (size_t i = 0; i < MAX_COUNT; ++i) {
+        generations[i] = i + 1;
+        if (objects[i] != nullptr)
+            intrusive_ptr_release(objects[i]);
+        objects[i] = nullptr;
+    }
+    next_free_slot = 0;
 }
 
 /// Initialize the kernel
 void Init() {
     Kernel::ThreadingInit();
-    Kernel::ArchiveInit();
 }
 
 /// Shutdown the kernel
 void Shutdown() {
     Kernel::ThreadingShutdown();
-    Kernel::ArchiveShutdown();
 
-    g_object_pool.Clear(); // Free all kernel objects
+    g_handle_table.Clear(); // Free all kernel objects
 }
 
 /**
@@ -106,8 +120,6 @@ void Shutdown() {
  * @return True on success, otherwise false
  */
 bool LoadExec(u32 entry_point) {
-    Init();
-
     Core::g_app_core->SetPC(entry_point);
 
     // 0x30 is the typical main thread priority I've seen used so far
diff --git a/src/core/hle/kernel/kernel.h b/src/core/hle/kernel/kernel.h
index 85e3264b9..7f86fd07d 100644
--- a/src/core/hle/kernel/kernel.h
+++ b/src/core/hle/kernel/kernel.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project / PPSSPP Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -12,9 +12,17 @@
 typedef u32 Handle;
 typedef s32 Result;
 
+const Handle INVALID_HANDLE = 0;
+
 namespace Kernel {
 
-enum KernelHandle {
+// TODO: Verify code
+const ResultCode ERR_OUT_OF_HANDLES(ErrorDescription::OutOfMemory, ErrorModule::Kernel,
+        ErrorSummary::OutOfResource, ErrorLevel::Temporary);
+// TOOD: Verify code
+const ResultCode ERR_INVALID_HANDLE = InvalidHandle(ErrorModule::Kernel);
+
+enum KernelHandle : Handle {
     CurrentThread   = 0xFFFF8000,
     CurrentProcess  = 0xFFFF8001,
 };
@@ -22,7 +30,7 @@ enum KernelHandle {
 enum class HandleType : u32 {
     Unknown         = 0,
     Port            = 1,
-    Service         = 2,
+    Session         = 2,
     Event           = 3,
     Mutex           = 4,
     SharedMemory    = 5,
@@ -30,20 +38,17 @@ enum class HandleType : u32 {
     Thread          = 7,
     Process         = 8,
     AddressArbiter  = 9,
-    File            = 10,
-    Semaphore       = 11,
-    Archive         = 12,
-    Directory       = 13,
+    Semaphore       = 10,
 };
 
 enum {
     DEFAULT_STACK_SIZE  = 0x4000,
 };
 
-class ObjectPool;
+class HandleTable;
 
 class Object : NonCopyable {
-    friend class ObjectPool;
+    friend class HandleTable;
     u32 handle;
 public:
     virtual ~Object() {}
@@ -53,15 +58,6 @@ public:
     virtual Kernel::HandleType GetHandleType() const = 0;
 
     /**
-     * Synchronize kernel object.
-     * @return True if the current thread should wait as a result of the sync
-     */
-    virtual ResultVal<bool> SyncRequest() {
-        LOG_ERROR(Kernel, "(UNIMPLEMENTED)");
-        return UnimplementedFunction(ErrorModule::Kernel);
-    }
-
-    /**
      * Wait for kernel object to synchronize.
      * @return True if the current thread should wait as a result of the wait
      */
@@ -69,100 +65,138 @@ public:
         LOG_ERROR(Kernel, "(UNIMPLEMENTED)");
         return UnimplementedFunction(ErrorModule::Kernel);
     }
+
+private:
+    friend void intrusive_ptr_add_ref(Object*);
+    friend void intrusive_ptr_release(Object*);
+
+    unsigned int ref_count = 0;
 };
 
-class ObjectPool : NonCopyable {
+// Special functions that will later be used by boost::instrusive_ptr to do automatic ref-counting
+inline void intrusive_ptr_add_ref(Object* object) {
+    ++object->ref_count;
+}
+
+inline void intrusive_ptr_release(Object* object) {
+    if (--object->ref_count == 0) {
+        delete object;
+    }
+}
+
+/**
+ * This class allows the creation of Handles, which are references to objects that can be tested
+ * for validity and looked up. Here they are used to pass references to kernel objects to/from the
+ * emulated process. it has been designed so that it follows the same handle format and has
+ * approximately the same restrictions as the handle manager in the CTR-OS.
+ *
+ * Handles contain two sub-fields: a slot index (bits 31:15) and a generation value (bits 14:0).
+ * The slot index is used to index into the arrays in this class to access the data corresponding
+ * to the Handle.
+ *
+ * To prevent accidental use of a freed Handle whose slot has already been reused, a global counter
+ * is kept and incremented every time a Handle is created. This is the Handle's "generation". The
+ * value of the counter is stored into the Handle as well as in the handle table (in the
+ * "generations" array). When looking up a handle, the Handle's generation must match with the
+ * value stored on the class, otherwise the Handle is considered invalid.
+ *
+ * To find free slots when allocating a Handle without needing to scan the entire object array, the
+ * generations field of unallocated slots is re-purposed as a linked list of indices to free slots.
+ * When a Handle is created, an index is popped off the list and used for the new Handle. When it
+ * is destroyed, it is again pushed onto the list to be re-used by the next allocation. It is
+ * likely that this allocation strategy differs from the one used in CTR-OS, but this hasn't been
+ * verified and isn't likely to cause any problems.
+ */
+class HandleTable final : NonCopyable {
 public:
-    ObjectPool();
-    ~ObjectPool() {}
+    HandleTable();
 
-    // Allocates a handle within the range and inserts the object into the map.
-    Handle Create(Object* obj, int range_bottom=INITIAL_NEXT_ID, int range_top=0x7FFFFFFF);
+    /**
+     * Allocates a handle for the given object.
+     * @return The created Handle or one of the following errors:
+     *           - `ERR_OUT_OF_HANDLES`: the maximum number of handles has been exceeded.
+     */
+    ResultVal<Handle> Create(Object* obj);
 
-    static Object* CreateByIDType(int type);
+    /**
+     * Returns a new handle that points to the same object as the passed in handle.
+     * @return The duplicated Handle or one of the following errors:
+     *           - `ERR_INVALID_HANDLE`: an invalid handle was passed in.
+     *           - Any errors returned by `Create()`.
+     */
+    ResultVal<Handle> Duplicate(Handle handle);
 
-    template <class T>
-    void Destroy(Handle handle) {
-        if (Get<T>(handle)) {
-            occupied[handle - HANDLE_OFFSET] = false;
-            delete pool[handle - HANDLE_OFFSET];
-        }
-    }
+    /**
+     * Closes a handle, removing it from the table and decreasing the object's ref-count.
+     * @return `RESULT_SUCCESS` or one of the following errors:
+     *           - `ERR_INVALID_HANDLE`: an invalid handle was passed in.
+     */
+    ResultCode Close(Handle handle);
 
+    /// Checks if a handle is valid and points to an existing object.
     bool IsValid(Handle handle) const;
 
-    template <class T>
-    T* Get(Handle handle) {
-        if (handle < HANDLE_OFFSET || handle >= HANDLE_OFFSET + MAX_COUNT || !occupied[handle - HANDLE_OFFSET]) {
-            if (handle != 0) {
-                LOG_ERROR(Kernel, "Bad object handle %08x", handle);
-            }
-            return nullptr;
-        } else {
-            Object* t = pool[handle - HANDLE_OFFSET];
-            if (t->GetHandleType() != T::GetStaticHandleType()) {
-                LOG_ERROR(Kernel, "Wrong object type for %08x", handle);
-                return nullptr;
-            }
-            return static_cast<T*>(t);
-        }
-    }
+    /**
+     * Looks up a handle.
+     * @returns Pointer to the looked-up object, or `nullptr` if the handle is not valid.
+     */
+    Object* GetGeneric(Handle handle) const;
 
-    // ONLY use this when you know the handle is valid.
+    /**
+     * Looks up a handle while verifying its type.
+     * @returns Pointer to the looked-up object, or `nullptr` if the handle is not valid or its
+     *          type differs from the handle type `T::HANDLE_TYPE`.
+     */
     template <class T>
-    T *GetFast(Handle handle) {
-        const Handle realHandle = handle - HANDLE_OFFSET;
-        _dbg_assert_(Kernel, realHandle >= 0 && realHandle < MAX_COUNT && occupied[realHandle]);
-        return static_cast<T*>(pool[realHandle]);
-    }
-
-    template <class T, typename ArgT>
-    void Iterate(bool func(T*, ArgT), ArgT arg) {
-        int type = T::GetStaticIDType();
-        for (int i = 0; i < MAX_COUNT; i++)
-        {
-            if (!occupied[i])
-                continue;
-            T* t = static_cast<T*>(pool[i]);
-            if (t->GetIDType() == type) {
-                if (!func(t, arg))
-                    break;
-            }
+    T* Get(Handle handle) const {
+        Object* object = GetGeneric(handle);
+        if (object != nullptr && object->GetHandleType() == T::HANDLE_TYPE) {
+            return static_cast<T*>(object);
         }
+        return nullptr;
     }
 
-    bool GetIDType(Handle handle, HandleType* type) const {
-        if ((handle < HANDLE_OFFSET) || (handle >= HANDLE_OFFSET + MAX_COUNT) ||
-                !occupied[handle - HANDLE_OFFSET]) {
-            LOG_ERROR(Kernel, "Bad object handle %08X", handle);
-            return false;
-        }
-        Object* t = pool[handle - HANDLE_OFFSET];
-        *type = t->GetHandleType();
-        return true;
-    }
-
-    Object* &operator [](Handle handle);
-    void List();
+    /// Closes all handles held in this table.
     void Clear();
-    int GetCount() const;
 
 private:
+    /**
+     * This is the maximum limit of handles allowed per process in CTR-OS. It can be further
+     * reduced by ExHeader values, but this is not emulated here.
+     */
+    static const size_t MAX_COUNT = 4096;
 
-    enum {
-        MAX_COUNT       = 0x1000,
-        HANDLE_OFFSET   = 0x100,
-        INITIAL_NEXT_ID = 0x10,
-    };
+    static size_t GetSlot(Handle handle)    { return handle >> 15; }
+    static u16 GetGeneration(Handle handle) { return handle & 0x7FFF; }
 
-    std::array<Object*, MAX_COUNT> pool;
-    std::array<bool, MAX_COUNT> occupied;
-    int next_id;
+    /// Stores the Object referenced by the handle or null if the slot is empty.
+    std::array<Object*, MAX_COUNT> objects;
+
+    /**
+     * The value of `next_generation` when the handle was created, used to check for validity. For
+     * empty slots, contains the index of the next free slot in the list.
+     */
+    std::array<u16, MAX_COUNT> generations;
+
+    /**
+     * Global counter of the number of created handles. Stored in `generations` when a handle is
+     * created, and wraps around to 1 when it hits 0x8000.
+     */
+    u16 next_generation;
+
+    /// Head of the free slots linked list.
+    u16 next_free_slot;
 };
 
-extern ObjectPool g_object_pool;
+extern HandleTable g_handle_table;
 extern Handle g_main_thread;
 
+/// The ID code of the currently running game
+/// TODO(Subv): This variable should not be here, 
+/// we need a way to store information about the currently loaded application 
+/// for later query during runtime, maybe using the LDR service?
+extern u64 g_program_id;
+
 /// Initialize the kernel
 void Init();
 
diff --git a/src/core/hle/kernel/mutex.cpp b/src/core/hle/kernel/mutex.cpp
index 5a173e129..558068c79 100644
--- a/src/core/hle/kernel/mutex.cpp
+++ b/src/core/hle/kernel/mutex.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include <map>
@@ -18,8 +18,8 @@ public:
     std::string GetTypeName() const override { return "Mutex"; }
     std::string GetName() const override { return name; }
 
-    static Kernel::HandleType GetStaticHandleType() { return Kernel::HandleType::Mutex; }
-    Kernel::HandleType GetHandleType() const override { return Kernel::HandleType::Mutex; }
+    static const HandleType HANDLE_TYPE = HandleType::Mutex;
+    HandleType GetHandleType() const override { return HANDLE_TYPE; }
 
     bool initial_locked;                        ///< Initial lock state when mutex was created
     bool locked;                                ///< Current locked state
@@ -87,7 +87,7 @@ void ReleaseThreadMutexes(Handle thread) {
     
     // Release every mutex that the thread holds, and resume execution on the waiting threads
     for (MutexMap::iterator iter = locked.first; iter != locked.second; ++iter) {
-        Mutex* mutex = g_object_pool.GetFast<Mutex>(iter->second);
+        Mutex* mutex = g_handle_table.Get<Mutex>(iter->second);
         ResumeWaitingThread(mutex);
     }
 
@@ -115,7 +115,7 @@ bool ReleaseMutex(Mutex* mutex) {
  * @param handle Handle to mutex to release
  */
 ResultCode ReleaseMutex(Handle handle) {
-    Mutex* mutex = Kernel::g_object_pool.Get<Mutex>(handle);
+    Mutex* mutex = Kernel::g_handle_table.Get<Mutex>(handle);
     if (mutex == nullptr) return InvalidHandle(ErrorModule::Kernel);
 
     if (!ReleaseMutex(mutex)) {
@@ -136,7 +136,8 @@ ResultCode ReleaseMutex(Handle handle) {
  */
 Mutex* CreateMutex(Handle& handle, bool initial_locked, const std::string& name) {
     Mutex* mutex = new Mutex;
-    handle = Kernel::g_object_pool.Create(mutex);
+    // TODO(yuriks): Fix error reporting
+    handle = Kernel::g_handle_table.Create(mutex).ValueOr(INVALID_HANDLE);
 
     mutex->locked = mutex->initial_locked = initial_locked;
     mutex->name = name;
diff --git a/src/core/hle/kernel/mutex.h b/src/core/hle/kernel/mutex.h
index 7f4909a6e..a8ca97014 100644
--- a/src/core/hle/kernel/mutex.h
+++ b/src/core/hle/kernel/mutex.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/core/hle/kernel/semaphore.cpp b/src/core/hle/kernel/semaphore.cpp
new file mode 100644
index 000000000..6bc8066a6
--- /dev/null
+++ b/src/core/hle/kernel/semaphore.cpp
@@ -0,0 +1,95 @@
+// Copyright 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#include <queue>
+
+#include "common/common.h"
+
+#include "core/hle/kernel/kernel.h"
+#include "core/hle/kernel/semaphore.h"
+#include "core/hle/kernel/thread.h"
+
+namespace Kernel {
+
+class Semaphore : public Object {
+public:
+    std::string GetTypeName() const override { return "Semaphore"; }
+    std::string GetName() const override { return name; }
+
+    static const HandleType HANDLE_TYPE = HandleType::Semaphore;
+    HandleType GetHandleType() const override { return HANDLE_TYPE; }
+
+    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)
+
+    /**
+     * Tests whether a semaphore still has free slots
+     * @return Whether the semaphore is available
+     */
+    bool IsAvailable() const {
+        return available_count > 0;
+    }
+
+    ResultVal<bool> WaitSynchronization() override {
+        bool wait = !IsAvailable();
+
+        if (wait) {
+            Kernel::WaitCurrentThread(WAITTYPE_SEMA, GetHandle());
+            waiting_threads.push(GetCurrentThreadHandle());
+        } else {
+            --available_count;
+        }
+
+        return MakeResult<bool>(wait);
+    }
+};
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+
+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,
+                          ErrorSummary::WrongArgument, ErrorLevel::Permanent);
+
+    Semaphore* semaphore = new Semaphore;
+    // TOOD(yuriks): Fix error reporting
+    *handle = g_handle_table.Create(semaphore).ValueOr(INVALID_HANDLE);
+
+    // When the semaphore is created, some slots are reserved for other threads,
+    // and the rest is reserved for the caller thread
+    semaphore->max_count = max_count;
+    semaphore->available_count = initial_count;
+    semaphore->name = name;
+
+    return RESULT_SUCCESS;
+}
+
+ResultCode ReleaseSemaphore(s32* count, Handle handle, s32 release_count) {
+    Semaphore* semaphore = g_handle_table.Get<Semaphore>(handle);
+    if (semaphore == nullptr)
+        return InvalidHandle(ErrorModule::Kernel);
+
+    if (semaphore->max_count - semaphore->available_count < release_count)
+        return ResultCode(ErrorDescription::OutOfRange, ErrorModule::Kernel, 
+                          ErrorSummary::InvalidArgument, ErrorLevel::Permanent);
+
+    *count = semaphore->available_count;
+    semaphore->available_count += release_count;
+
+    // Notify some of the threads that the semaphore has been released
+    // stop once the semaphore is full again or there are no more waiting threads
+    while (!semaphore->waiting_threads.empty() && semaphore->IsAvailable()) {
+        Kernel::ResumeThreadFromWait(semaphore->waiting_threads.front());
+        semaphore->waiting_threads.pop();
+        --semaphore->available_count;
+    }
+
+    return RESULT_SUCCESS;
+}
+
+} // namespace
diff --git a/src/core/hle/kernel/semaphore.h b/src/core/hle/kernel/semaphore.h
new file mode 100644
index 000000000..8644ecf0c
--- /dev/null
+++ b/src/core/hle/kernel/semaphore.h
@@ -0,0 +1,32 @@
+// Copyright 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#pragma once
+
+#include "common/common_types.h"
+
+#include "core/hle/kernel/kernel.h"
+
+namespace Kernel {
+
+/**
+ * Creates a semaphore.
+ * @param handle Pointer to the handle of the newly created object
+ * @param initial_count Number of slots reserved for other threads
+ * @param max_count Maximum number of slots the semaphore can have
+ * @param name Optional name of semaphore
+ * @return ResultCode of the error
+ */
+ResultCode CreateSemaphore(Handle* handle, s32 initial_count, s32 max_count, const std::string& name = "Unknown");
+
+/**
+ * Releases a certain number of slots from a semaphore.
+ * @param count The number of free slots the semaphore had before this call
+ * @param handle The handle of the semaphore to release
+ * @param release_count The number of slots to release
+ * @return ResultCode of the error
+ */
+ResultCode ReleaseSemaphore(s32* count, Handle handle, s32 release_count);
+
+} // namespace
diff --git a/src/core/hle/kernel/session.h b/src/core/hle/kernel/session.h
new file mode 100644
index 000000000..91f3ffc2c
--- /dev/null
+++ b/src/core/hle/kernel/session.h
@@ -0,0 +1,58 @@
+// Copyright 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#pragma once
+
+#include "core/hle/kernel/kernel.h"
+
+namespace Kernel {
+
+static const int kCommandHeaderOffset = 0x80; ///< Offset into command buffer of header
+
+/**
+ * Returns a pointer to the command buffer in kernel memory
+ * @param offset Optional offset into command buffer
+ * @return Pointer to command buffer
+ */
+inline static u32* GetCommandBuffer(const int offset=0) {
+    return (u32*)Memory::GetPointer(Memory::KERNEL_MEMORY_VADDR + kCommandHeaderOffset + offset);
+}
+
+/**
+ * Kernel object representing the client endpoint of an IPC session. Sessions are the basic CTR-OS
+ * primitive for communication between different processes, and are used to implement service calls
+ * to the various system services.
+ *
+ * To make a service call, the client must write the command header and parameters to the buffer
+ * located at offset 0x80 of the TLS (Thread-Local Storage) area, then execute a SendSyncRequest
+ * SVC call with its Session handle. The kernel will read the command header, using it to marshall
+ * the parameters to the process at the server endpoint of the session. After the server replies to
+ * the request, the response is marshalled back to the caller's TLS buffer and control is
+ * transferred back to it.
+ *
+ * In Citra, only the client endpoint is currently implemented and only HLE calls, where the IPC
+ * request is answered by C++ code in the emulator, are supported. When SendSyncRequest is called
+ * with the session handle, this class's SyncRequest method is called, which should read the TLS
+ * buffer and emulate the call accordingly. Since the code can directly read the emulated memory,
+ * no parameter marshalling is done.
+ *
+ * In the long term, this should be turned into the full-fledged IPC mechanism implemented by
+ * CTR-OS so that IPC calls can be optionally handled by the real implementations of processes, as
+ * opposed to HLE simulations.
+ */
+class Session : public Object {
+public:
+    std::string GetTypeName() const override { return "Session"; }
+
+    static const HandleType HANDLE_TYPE = HandleType::Session;
+    HandleType GetHandleType() const override { return HANDLE_TYPE; }
+
+    /**
+     * Handles a synchronous call to this session using HLE emulation. Emulated <-> emulated calls
+     * aren't supported yet.
+     */
+    virtual ResultVal<bool> SyncRequest() = 0;
+};
+
+}
diff --git a/src/core/hle/kernel/shared_memory.cpp b/src/core/hle/kernel/shared_memory.cpp
index 3c8c502c6..cea1f6fa1 100644
--- a/src/core/hle/kernel/shared_memory.cpp
+++ b/src/core/hle/kernel/shared_memory.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "common/common.h"
@@ -13,8 +13,8 @@ class SharedMemory : public Object {
 public:
     std::string GetTypeName() const override { return "SharedMemory"; }
 
-    static Kernel::HandleType GetStaticHandleType() {  return Kernel::HandleType::SharedMemory; }
-    Kernel::HandleType GetHandleType() const override { return Kernel::HandleType::SharedMemory; }
+    static const HandleType HANDLE_TYPE = HandleType::SharedMemory;
+    HandleType GetHandleType() const override { return HANDLE_TYPE; }
 
     u32 base_address;                   ///< Address of shared memory block in RAM
     MemoryPermission permissions;       ///< Permissions of shared memory block (SVC field)
@@ -32,7 +32,8 @@ public:
  */
 SharedMemory* CreateSharedMemory(Handle& handle, const std::string& name) {
     SharedMemory* shared_memory = new SharedMemory;
-    handle = Kernel::g_object_pool.Create(shared_memory);
+    // TOOD(yuriks): Fix error reporting
+    handle = Kernel::g_handle_table.Create(shared_memory).ValueOr(INVALID_HANDLE);
     shared_memory->name = name;
     return shared_memory;
 }
@@ -60,7 +61,7 @@ ResultCode MapSharedMemory(u32 handle, u32 address, MemoryPermission permissions
         return ResultCode(ErrorDescription::InvalidAddress, ErrorModule::Kernel,
                 ErrorSummary::InvalidArgument, ErrorLevel::Permanent);
     }
-    SharedMemory* shared_memory = Kernel::g_object_pool.Get<SharedMemory>(handle);
+    SharedMemory* shared_memory = Kernel::g_handle_table.Get<SharedMemory>(handle);
     if (shared_memory == nullptr) return InvalidHandle(ErrorModule::Kernel);
 
     shared_memory->base_address = address;
@@ -71,7 +72,7 @@ ResultCode MapSharedMemory(u32 handle, u32 address, MemoryPermission permissions
 }
 
 ResultVal<u8*> GetSharedMemoryPointer(Handle handle, u32 offset) {
-    SharedMemory* shared_memory = Kernel::g_object_pool.Get<SharedMemory>(handle);
+    SharedMemory* shared_memory = Kernel::g_handle_table.Get<SharedMemory>(handle);
     if (shared_memory == nullptr) return InvalidHandle(ErrorModule::Kernel);
 
     if (0 != shared_memory->base_address)
diff --git a/src/core/hle/kernel/shared_memory.h b/src/core/hle/kernel/shared_memory.h
index 304cf5b67..bb65c7ccd 100644
--- a/src/core/hle/kernel/shared_memory.h
+++ b/src/core/hle/kernel/shared_memory.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -12,11 +12,15 @@ namespace Kernel {
 
 /// Permissions for mapped shared memory blocks
 enum class MemoryPermission : u32 {
-    None        = 0,
-    Read        = (1u <<  0),
-    Write       = (1u <<  1),
-    ReadWrite   = (Read | Write),
-    DontCare    = (1u << 28)
+    None             = 0,
+    Read             = (1u <<  0),
+    Write            = (1u <<  1),
+    ReadWrite        = (Read | Write),
+    Execute          = (1u <<  2),
+    ReadExecute      = (Read | Execute),
+    WriteExecute     = (Write | Execute),
+    ReadWriteExecute = (Read | Write | Execute),
+    DontCare         = (1u << 28)
 };
 
 /**
diff --git a/src/core/hle/kernel/thread.cpp b/src/core/hle/kernel/thread.cpp
index 1c04701de..872df2d14 100644
--- a/src/core/hle/kernel/thread.cpp
+++ b/src/core/hle/kernel/thread.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project / PPSSPP Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include <algorithm>
@@ -26,8 +26,8 @@ public:
     std::string GetName() const override { return name; }
     std::string GetTypeName() const override { return "Thread"; }
 
-    static Kernel::HandleType GetStaticHandleType() { return Kernel::HandleType::Thread; }
-    Kernel::HandleType GetHandleType() const override { return Kernel::HandleType::Thread; }
+    static const HandleType HANDLE_TYPE = HandleType::Thread;
+    HandleType GetHandleType() const override { return HANDLE_TYPE; }
 
     inline bool IsRunning() const { return (status & THREADSTATUS_RUNNING) != 0; }
     inline bool IsStopped() const { return (status & THREADSTATUS_DORMANT) != 0; }
@@ -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,21 +147,24 @@ 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
 ResultCode StopThread(Handle handle, const char* reason) {
-    Thread* thread = g_object_pool.Get<Thread>(handle);
+    Thread* thread = g_handle_table.Get<Thread>(handle);
     if (thread == nullptr) return InvalidHandle(ErrorModule::Kernel);
 
     // Release all the mutexes that this thread holds
@@ -171,10 +173,10 @@ ResultCode StopThread(Handle handle, const char* reason) {
     ChangeReadyState(thread, false);
     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)) {
+        Thread* waiting_thread = g_handle_table.Get<Thread>(waiting_handle);
+
+        if (CheckWaitType(waiting_thread, WAITTYPE_THREADEND, handle))
             ResumeThreadFromWait(waiting_handle);
-        }
     }
     thread->waiting_threads.clear();
 
@@ -208,9 +210,9 @@ Handle ArbitrateHighestPriorityThread(u32 arbiter, u32 address) {
 
     // Iterate through threads, find highest priority thread that is waiting to be arbitrated...
     for (Handle handle : thread_queue) {
-        Thread* thread = g_object_pool.Get<Thread>(handle);
+        Thread* thread = g_handle_table.Get<Thread>(handle);
 
-        if (!VerifyWait(thread, WAITTYPE_ARB, arbiter, address))
+        if (!CheckWaitType(thread, WAITTYPE_ARB, arbiter, address))
             continue;
 
         if (thread == nullptr)
@@ -233,9 +235,9 @@ void ArbitrateAllThreads(u32 arbiter, u32 address) {
 
     // Iterate through threads, find highest priority thread that is waiting to be arbitrated...
     for (Handle handle : thread_queue) {
-        Thread* thread = g_object_pool.Get<Thread>(handle);
+        Thread* thread = g_handle_table.Get<Thread>(handle);
 
-        if (VerifyWait(thread, WAITTYPE_ARB, arbiter, address))
+        if (CheckWaitType(thread, WAITTYPE_ARB, arbiter, address))
             ResumeThreadFromWait(handle);
     }
 }
@@ -286,7 +288,7 @@ Thread* NextThread() {
     if (next == 0) {
         return nullptr;
     }
-    return Kernel::g_object_pool.Get<Thread>(next);
+    return Kernel::g_handle_table.Get<Thread>(next);
 }
 
 void WaitCurrentThread(WaitType wait_type, Handle wait_handle) {
@@ -303,9 +305,11 @@ void WaitCurrentThread(WaitType wait_type, Handle wait_handle, VAddr wait_addres
 
 /// Resumes a thread from waiting by marking it as "ready"
 void ResumeThreadFromWait(Handle handle) {
-    Thread* thread = Kernel::g_object_pool.Get<Thread>(handle);
+    Thread* thread = Kernel::g_handle_table.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);
         }
@@ -337,7 +341,8 @@ Thread* CreateThread(Handle& handle, const char* name, u32 entry_point, s32 prio
 
     Thread* thread = new Thread;
 
-    handle = Kernel::g_object_pool.Create(thread);
+    // TOOD(yuriks): Fix error reporting
+    handle = Kernel::g_handle_table.Create(thread).ValueOr(INVALID_HANDLE);
 
     thread_queue.push_back(handle);
     thread_ready_queue.prepare(priority);
@@ -394,7 +399,7 @@ Handle CreateThread(const char* name, u32 entry_point, s32 priority, u32 arg, s3
 
 /// Get the priority of the thread specified by handle
 ResultVal<u32> GetThreadPriority(const Handle handle) {
-    Thread* thread = g_object_pool.Get<Thread>(handle);
+    Thread* thread = g_handle_table.Get<Thread>(handle);
     if (thread == nullptr) return InvalidHandle(ErrorModule::Kernel);
 
     return MakeResult<u32>(thread->current_priority);
@@ -406,7 +411,7 @@ ResultCode SetThreadPriority(Handle handle, s32 priority) {
     if (!handle) {
         thread = GetCurrentThread(); // TODO(bunnei): Is this correct behavior?
     } else {
-        thread = g_object_pool.Get<Thread>(handle);
+        thread = g_handle_table.Get<Thread>(handle);
         if (thread == nullptr) {
             return InvalidHandle(ErrorModule::Kernel);
         }
@@ -469,23 +474,31 @@ 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_handle_table.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) {
-    Thread* thread = g_object_pool.Get<Thread>(handle);
+    Thread* thread = g_handle_table.Get<Thread>(handle);
     if (thread == nullptr)
         return ResultCode(ErrorDescription::InvalidHandle, ErrorModule::OS, 
                           ErrorSummary::WrongArgument, ErrorLevel::Permanent);
diff --git a/src/core/hle/kernel/thread.h b/src/core/hle/kernel/thread.h
index be7adface..0e1397cd9 100644
--- a/src/core/hle/kernel/thread.h
+++ b/src/core/hle/kernel/thread.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project / PPSSPP Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -40,7 +40,6 @@ enum WaitType {
     WAITTYPE_SEMA,
     WAITTYPE_EVENT,
     WAITTYPE_THREADEND,
-    WAITTYPE_VBLANK,
     WAITTYPE_MUTEX,
     WAITTYPE_SYNCH,
     WAITTYPE_ARB,
diff --git a/src/core/hle/result.h b/src/core/hle/result.h
index 15c4a2677..0e9c213e0 100644
--- a/src/core/hle/result.h
+++ b/src/core/hle/result.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -17,6 +17,8 @@
 /// Detailed description of the error. This listing is likely incomplete.
 enum class ErrorDescription : u32 {
     Success = 0,
+    FS_NotFound = 100,
+    FS_NotFormatted = 340, ///< This is used by the FS service when creating a SaveData archive
     InvalidSection = 1000,
     TooLarge = 1001,
     NotAuthorized = 1002,
diff --git a/src/core/hle/service/ac_u.cpp b/src/core/hle/service/ac_u.cpp
index 4130feb9d..20a3fa2e5 100644
--- a/src/core/hle/service/ac_u.cpp
+++ b/src/core/hle/service/ac_u.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "common/log.h"
@@ -18,7 +18,7 @@ namespace AC_U {
  *      2 : Output connection type, 0 = none, 1 = Old3DS Internet, 2 = New3DS Internet.
  */
 void GetWifiStatus(Service::Interface* self) {
-    u32* cmd_buff = Service::GetCommandBuffer();
+    u32* cmd_buff = Kernel::GetCommandBuffer();
 
     // TODO(purpasmart96): This function is only a stub,
     // it returns a valid result without implementing full functionality.
@@ -56,7 +56,4 @@ Interface::Interface() {
     Register(FunctionTable, ARRAY_SIZE(FunctionTable));
 }
 
-Interface::~Interface() {
-}
-
 } // namespace
diff --git a/src/core/hle/service/ac_u.h b/src/core/hle/service/ac_u.h
index c91b28353..f1d26ebe8 100644
--- a/src/core/hle/service/ac_u.h
+++ b/src/core/hle/service/ac_u.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -16,11 +16,7 @@ namespace AC_U {
 class Interface : public Service::Interface {
 public:
     Interface();
-    ~Interface();
-    /**
-     * Gets the string port name used by CTROS for the service
-     * @return Port name of service
-     */
+
     std::string GetPortName() const override {
         return "ac:u";
     }
diff --git a/src/core/hle/service/act_u.cpp b/src/core/hle/service/act_u.cpp
new file mode 100644
index 000000000..10870f14b
--- /dev/null
+++ b/src/core/hle/service/act_u.cpp
@@ -0,0 +1,24 @@
+// Copyright 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#include "common/log.h"
+#include "core/hle/hle.h"
+#include "core/hle/service/act_u.h"
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// Namespace ACT_U
+
+namespace ACT_U {
+
+// Empty arrays are illegal -- commented out until an entry is added.
+//const Interface::FunctionInfo FunctionTable[] = { };
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// Interface class
+
+Interface::Interface() {
+    //Register(FunctionTable, ARRAY_SIZE(FunctionTable));
+}
+
+} // namespace
diff --git a/src/core/hle/service/act_u.h b/src/core/hle/service/act_u.h
new file mode 100644
index 000000000..be41454a4
--- /dev/null
+++ b/src/core/hle/service/act_u.h
@@ -0,0 +1,23 @@
+// Copyright 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#pragma once
+
+#include "core/hle/service/service.h"
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// Namespace ACT_U
+
+namespace ACT_U {
+
+class Interface : public Service::Interface {
+public:
+    Interface();
+
+    std::string GetPortName() const override {
+        return "act:u";
+    }
+};
+
+} // namespace
diff --git a/src/core/hle/service/am_app.cpp b/src/core/hle/service/am_app.cpp
new file mode 100644
index 000000000..0b396b6d3
--- /dev/null
+++ b/src/core/hle/service/am_app.cpp
@@ -0,0 +1,24 @@
+// Copyright 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#include "common/log.h"
+#include "core/hle/hle.h"
+#include "core/hle/service/am_app.h"
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// Namespace AM_APP
+
+namespace AM_APP {
+
+// Empty arrays are illegal -- commented out until an entry is added.
+//const Interface::FunctionInfo FunctionTable[] = { };
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// Interface class
+
+Interface::Interface() {
+    //Register(FunctionTable, ARRAY_SIZE(FunctionTable));
+}
+
+} // namespace
diff --git a/src/core/hle/service/am_app.h b/src/core/hle/service/am_app.h
new file mode 100644
index 000000000..50dc2f5a2
--- /dev/null
+++ b/src/core/hle/service/am_app.h
@@ -0,0 +1,23 @@
+// Copyright 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#pragma once
+
+#include "core/hle/service/service.h"
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// Namespace AM_APP
+
+namespace AM_APP {
+
+class Interface : public Service::Interface {
+public:
+    Interface();
+
+    std::string GetPortName() const override {
+        return "am:app";
+    }
+};
+
+} // namespace
diff --git a/src/core/hle/service/am_net.cpp b/src/core/hle/service/am_net.cpp
index 403cac353..112844e5b 100644
--- a/src/core/hle/service/am_net.cpp
+++ b/src/core/hle/service/am_net.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "common/log.h"
@@ -41,7 +41,4 @@ Interface::Interface() {
     Register(FunctionTable, ARRAY_SIZE(FunctionTable));
 }
 
-Interface::~Interface() {
-}
-
 } // namespace
diff --git a/src/core/hle/service/am_net.h b/src/core/hle/service/am_net.h
index 4816e1697..616c33ee8 100644
--- a/src/core/hle/service/am_net.h
+++ b/src/core/hle/service/am_net.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -14,11 +14,7 @@ namespace AM_NET {
 class Interface : public Service::Interface {
 public:
     Interface();
-    ~Interface();
-    /**
-     * Gets the string port name used by CTROS for the service
-     * @return Port name of service
-     */
+
     std::string GetPortName() const override {
         return "am:net";
     }
diff --git a/src/core/hle/service/apt_a.cpp b/src/core/hle/service/apt_a.cpp
new file mode 100644
index 000000000..dcf5ec4fe
--- /dev/null
+++ b/src/core/hle/service/apt_a.cpp
@@ -0,0 +1,34 @@
+// Copyright 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#include "common/log.h"
+#include "core/hle/hle.h"
+#include "core/hle/service/apt_a.h"
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// Namespace APT_A
+
+namespace APT_A {
+
+const Interface::FunctionInfo FunctionTable[] = {
+    {0x00010040, nullptr,               "GetLockHandle?"},
+    {0x00020080, nullptr,               "Initialize?"},
+    {0x00030040, nullptr,               "Enable?"},
+    {0x00040040, nullptr,               "Finalize?"},
+    {0x00050040, nullptr,               "GetAppletManInfo?"},
+    {0x00060040, nullptr,               "GetAppletInfo?"},
+    {0x003B0040, nullptr,               "CancelLibraryApplet?"},
+    {0x00430040, nullptr,               "NotifyToWait?"},
+    {0x004B00C2, nullptr,               "AppletUtility?"},
+    {0x00550040, nullptr,               "WriteInputToNsState?"},
+};
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// Interface class
+
+Interface::Interface() {
+    Register(FunctionTable, ARRAY_SIZE(FunctionTable));
+}
+
+} // namespace
diff --git a/src/core/hle/service/apt_a.h b/src/core/hle/service/apt_a.h
new file mode 100644
index 000000000..6cbf1288f
--- /dev/null
+++ b/src/core/hle/service/apt_a.h
@@ -0,0 +1,23 @@
+// Copyright 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#pragma once
+
+#include "core/hle/service/service.h"
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// Namespace APT_A
+
+namespace APT_A {
+
+class Interface : public Service::Interface {
+public:
+    Interface();
+
+    std::string GetPortName() const override {
+        return "APT:A";
+    }
+};
+
+} // namespace
diff --git a/src/core/hle/service/apt_u.cpp b/src/core/hle/service/apt_u.cpp
index b6d5d101f..d8b261ba7 100644
--- a/src/core/hle/service/apt_u.cpp
+++ b/src/core/hle/service/apt_u.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 
@@ -40,7 +40,7 @@ enum class SignalType : u32 {
 };
 
 void Initialize(Service::Interface* self) {
-    u32* cmd_buff = Service::GetCommandBuffer();
+    u32* cmd_buff = Kernel::GetCommandBuffer();
 
     cmd_buff[3] = Kernel::CreateEvent(RESETTYPE_ONESHOT, "APT_U:Menu");  // APT menu event handle
     cmd_buff[4] = Kernel::CreateEvent(RESETTYPE_ONESHOT, "APT_U:Pause"); // APT pause event handle
@@ -57,7 +57,7 @@ void Initialize(Service::Interface* self) {
 }
 
 void GetLockHandle(Service::Interface* self) {
-    u32* cmd_buff = Service::GetCommandBuffer();
+    u32* cmd_buff = Kernel::GetCommandBuffer();
     u32 flags = cmd_buff[1]; // TODO(bunnei): Figure out the purpose of the flag field
 
     if (0 == lock_handle) {
@@ -78,14 +78,14 @@ void GetLockHandle(Service::Interface* self) {
 }
 
 void Enable(Service::Interface* self) {
-    u32* cmd_buff = Service::GetCommandBuffer();
+    u32* cmd_buff = Kernel::GetCommandBuffer();
     u32 unk = cmd_buff[1]; // TODO(bunnei): What is this field used for?
     cmd_buff[1] = 0; // No error
     LOG_WARNING(Service_APT, "(STUBBED) called unk=0x%08X", unk);
 }
 
 void InquireNotification(Service::Interface* self) {
-    u32* cmd_buff = Service::GetCommandBuffer();
+    u32* cmd_buff = Kernel::GetCommandBuffer();
     u32 app_id = cmd_buff[2];
     cmd_buff[1] = 0; // No error
     cmd_buff[2] = static_cast<u32>(SignalType::None); // Signal type
@@ -112,7 +112,7 @@ void InquireNotification(Service::Interface* self) {
  *      8 : Output parameter buffer ptr
  */
 void ReceiveParameter(Service::Interface* self) {
-    u32* cmd_buff = Service::GetCommandBuffer();
+    u32* cmd_buff = Kernel::GetCommandBuffer();
     u32 app_id = cmd_buff[1];
     u32 buffer_size = cmd_buff[2];
     cmd_buff[1] = 0; // No error
@@ -143,7 +143,7 @@ void ReceiveParameter(Service::Interface* self) {
  *      8 : Output parameter buffer ptr
  */
 void GlanceParameter(Service::Interface* self) {
-    u32* cmd_buff = Service::GetCommandBuffer();
+    u32* cmd_buff = Kernel::GetCommandBuffer();
     u32 app_id = cmd_buff[1];
     u32 buffer_size = cmd_buff[2];
 
@@ -170,7 +170,7 @@ void GlanceParameter(Service::Interface* self) {
  *      1 : Result of function, 0 on success, otherwise error code
  */
 void AppletUtility(Service::Interface* self) {
-    u32* cmd_buff = Service::GetCommandBuffer();
+    u32* cmd_buff = Kernel::GetCommandBuffer();
 
     // These are from 3dbrew - I'm not really sure what they're used for.
     u32 unk = cmd_buff[1];
@@ -196,7 +196,7 @@ void AppletUtility(Service::Interface* self) {
 void GetSharedFont(Service::Interface* self) {
     LOG_TRACE(Kernel_SVC, "called");
 
-    u32* cmd_buff = Service::GetCommandBuffer();
+    u32* cmd_buff = Kernel::GetCommandBuffer();
 
     if (!shared_font.empty()) {
         // TODO(bunnei): This function shouldn't copy the shared font every time it's called.
@@ -315,8 +315,8 @@ Interface::Interface() {
 
     if (file.IsOpen()) {
         // Read shared font data
-        shared_font.resize(file.GetSize());
-        file.ReadBytes(shared_font.data(), file.GetSize());
+        shared_font.resize((size_t)file.GetSize());
+        file.ReadBytes(shared_font.data(), (size_t)file.GetSize());
 
         // Create shared font memory object
         shared_font_mem = Kernel::CreateSharedMemory("APT_U:shared_font_mem");
@@ -330,7 +330,4 @@ Interface::Interface() {
     Register(FunctionTable, ARRAY_SIZE(FunctionTable));
 }
 
-Interface::~Interface() {
-}
-
 } // namespace
diff --git a/src/core/hle/service/apt_u.h b/src/core/hle/service/apt_u.h
index 306730400..aad918cfc 100644
--- a/src/core/hle/service/apt_u.h
+++ b/src/core/hle/service/apt_u.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -20,15 +20,8 @@ namespace APT_U {
 /// Interface to "APT:U" service
 class Interface : public Service::Interface {
 public:
-
     Interface();
 
-    ~Interface();
-
-    /**
-     * Gets the string port name used by CTROS for the service
-     * @return Port name of service
-     */
     std::string GetPortName() const override {
         return "APT:U";
     }
diff --git a/src/core/hle/service/boss_u.cpp b/src/core/hle/service/boss_u.cpp
index b2ff4a756..1820ea7ad 100644
--- a/src/core/hle/service/boss_u.cpp
+++ b/src/core/hle/service/boss_u.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "common/log.h"
@@ -11,18 +11,15 @@
 
 namespace BOSS_U {
 
-    const Interface::FunctionInfo FunctionTable[] = {
-        {0x00020100, nullptr,               "GetStorageInfo"},
-    };
+const Interface::FunctionInfo FunctionTable[] = {
+    {0x00020100, nullptr,               "GetStorageInfo"},
+};
 
-    ////////////////////////////////////////////////////////////////////////////////////////////////////
-    // Interface class
-
-    Interface::Interface() {
-        Register(FunctionTable, ARRAY_SIZE(FunctionTable));
-    }
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// Interface class
 
-    Interface::~Interface() {
-    }
+Interface::Interface() {
+    Register(FunctionTable, ARRAY_SIZE(FunctionTable));
+}
 
 } // namespace
diff --git a/src/core/hle/service/boss_u.h b/src/core/hle/service/boss_u.h
index af39b8e65..2668f2dfd 100644
--- a/src/core/hle/service/boss_u.h
+++ b/src/core/hle/service/boss_u.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -11,17 +11,13 @@
 
 namespace BOSS_U {
 
-    class Interface : public Service::Interface {
-    public:
-        Interface();
-        ~Interface();
-        /**
-         * Gets the string port name used by CTROS for the service
-         * @return Port name of service
-         */
-        std::string GetPortName() const {
-            return "boss:U";
-        }
-    };
+class Interface : public Service::Interface {
+public:
+    Interface();
+    
+    std::string GetPortName() const override {
+        return "boss:U";
+    }
+};
 
 } // namespace
diff --git a/src/core/hle/service/cecd_u.cpp b/src/core/hle/service/cecd_u.cpp
new file mode 100644
index 000000000..b7655ef0b
--- /dev/null
+++ b/src/core/hle/service/cecd_u.cpp
@@ -0,0 +1,24 @@
+// Copyright 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#include "common/log.h"
+#include "core/hle/hle.h"
+#include "core/hle/service/cecd_u.h"
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// Namespace CECD_U
+
+namespace CECD_U {
+
+// Empty arrays are illegal -- commented out until an entry is added.
+//const Interface::FunctionInfo FunctionTable[] = { };
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// Interface class
+
+Interface::Interface() {
+    //Register(FunctionTable, ARRAY_SIZE(FunctionTable));
+}
+
+} // namespace
diff --git a/src/core/hle/service/cecd_u.h b/src/core/hle/service/cecd_u.h
new file mode 100644
index 000000000..e67564135
--- /dev/null
+++ b/src/core/hle/service/cecd_u.h
@@ -0,0 +1,23 @@
+// Copyright 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#pragma once
+
+#include "core/hle/service/service.h"
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// Namespace CECD_U
+
+namespace CECD_U {
+
+class Interface : public Service::Interface {
+public:
+    Interface();
+
+    std::string GetPortName() const override {
+        return "cecd:u";
+    }
+};
+
+} // namespace
diff --git a/src/core/hle/service/cfg/cfg.cpp b/src/core/hle/service/cfg/cfg.cpp
new file mode 100644
index 000000000..161aa8531
--- /dev/null
+++ b/src/core/hle/service/cfg/cfg.cpp
@@ -0,0 +1,202 @@
+// Copyright 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#include <algorithm>
+#include "common/log.h"
+#include "common/make_unique.h"
+#include "core/file_sys/archive_systemsavedata.h"
+#include "core/hle/service/cfg/cfg.h"
+
+namespace Service {
+namespace CFG {
+
+const u64 CFG_SAVE_ID = 0x00010017;
+const u64 CONSOLE_UNIQUE_ID = 0xDEADC0DE;
+const ConsoleModelInfo CONSOLE_MODEL = { NINTENDO_3DS_XL, { 0, 0, 0 } };
+const u8 CONSOLE_LANGUAGE = LANGUAGE_EN;
+const char CONSOLE_USERNAME[0x14] = "CITRA";
+/// This will be initialized in CFGInit, and will be used when creating the block
+UsernameBlock CONSOLE_USERNAME_BLOCK;
+/// TODO(Subv): Find out what this actually is
+const u8 SOUND_OUTPUT_MODE = 2;
+const u8 UNITED_STATES_COUNTRY_ID = 49;
+/// TODO(Subv): Find what the other bytes are
+const ConsoleCountryInfo COUNTRY_INFO = { { 0, 0, 0 }, UNITED_STATES_COUNTRY_ID };
+
+/**
+ * TODO(Subv): Find out what this actually is, these values fix some NaN uniforms in some games,
+ * for example Nintendo Zone
+ * Thanks Normmatt for providing this information
+ */
+const std::array<float, 8> STEREO_CAMERA_SETTINGS = {
+    62.0f, 289.0f, 76.80000305175781f, 46.08000183105469f,
+    10.0f, 5.0f, 55.58000183105469f, 21.56999969482422f
+};
+
+static const u32 CONFIG_SAVEFILE_SIZE = 0x8000;
+static std::array<u8, CONFIG_SAVEFILE_SIZE> cfg_config_file_buffer;
+
+static std::unique_ptr<FileSys::Archive_SystemSaveData> cfg_system_save_data;
+
+ResultCode GetConfigInfoBlock(u32 block_id, u32 size, u32 flag, u8* output) {
+    // Read the header
+    SaveFileConfig* config = reinterpret_cast<SaveFileConfig*>(cfg_config_file_buffer.data());
+
+    auto itr = std::find_if(std::begin(config->block_entries), std::end(config->block_entries),
+            [&](const SaveConfigBlockEntry& entry) {
+                return entry.block_id == block_id && entry.size == size && (entry.flags & flag);
+            });
+
+    if (itr == std::end(config->block_entries)) {
+        LOG_ERROR(Service_CFG, "Config block %u with size %u and flags %u not found", block_id, size, flag);
+        return ResultCode(-1); // TODO(Subv): Find the correct error code
+    }
+
+    // The data is located in the block header itself if the size is less than 4 bytes
+    if (itr->size <= 4)
+        memcpy(output, &itr->offset_or_data, itr->size);
+    else
+        memcpy(output, &cfg_config_file_buffer[itr->offset_or_data], itr->size);
+
+    return RESULT_SUCCESS;
+}
+
+ResultCode CreateConfigInfoBlk(u32 block_id, u16 size, u16 flags, const u8* data) {
+    SaveFileConfig* config = reinterpret_cast<SaveFileConfig*>(cfg_config_file_buffer.data());
+    if (config->total_entries >= CONFIG_FILE_MAX_BLOCK_ENTRIES)
+        return ResultCode(-1); // TODO(Subv): Find the right error code
+
+    // Insert the block header with offset 0 for now
+    config->block_entries[config->total_entries] = { block_id, 0, size, flags };
+    if (size > 4) {
+        u32 offset = config->data_entries_offset;
+        // Perform a search to locate the next offset for the new data
+        // use the offset and size of the previous block to determine the new position
+        for (int i = config->total_entries - 1; i >= 0; --i) {
+            // Ignore the blocks that don't have a separate data offset
+            if (config->block_entries[i].size > 4) {
+                offset = config->block_entries[i].offset_or_data +
+                         config->block_entries[i].size;
+                break;
+            }
+        }
+
+        config->block_entries[config->total_entries].offset_or_data = offset;
+
+        // Write the data at the new offset
+        memcpy(&cfg_config_file_buffer[offset], data, size);
+    }
+    else {
+        // The offset_or_data field in the header contains the data itself if it's 4 bytes or less
+        memcpy(&config->block_entries[config->total_entries].offset_or_data, data, size);
+    }
+
+    ++config->total_entries;
+    return RESULT_SUCCESS;
+}
+
+ResultCode DeleteConfigNANDSaveFile() {
+    FileSys::Path path("config");
+    if (cfg_system_save_data->DeleteFile(path))
+        return RESULT_SUCCESS;
+    return ResultCode(-1); // TODO(Subv): Find the right error code
+}
+
+ResultCode UpdateConfigNANDSavegame() {
+    FileSys::Mode mode = {};
+    mode.write_flag = 1;
+    mode.create_flag = 1;
+    FileSys::Path path("config");
+    auto file = cfg_system_save_data->OpenFile(path, mode);
+    _assert_msg_(Service_CFG, file != nullptr, "could not open file");
+    file->Write(0, CONFIG_SAVEFILE_SIZE, 1, cfg_config_file_buffer.data());
+    return RESULT_SUCCESS;
+}
+
+ResultCode FormatConfig() {
+    ResultCode res = DeleteConfigNANDSaveFile();
+    if (!res.IsSuccess())
+        return res;
+    // Delete the old data
+    cfg_config_file_buffer.fill(0);
+    // Create the header
+    SaveFileConfig* config = reinterpret_cast<SaveFileConfig*>(cfg_config_file_buffer.data());
+    // This value is hardcoded, taken from 3dbrew, verified by hardware, it's always the same value
+    config->data_entries_offset = 0x455C;
+    // Insert the default blocks
+    res = CreateConfigInfoBlk(0x00050005, sizeof(STEREO_CAMERA_SETTINGS), 0xE,
+                              reinterpret_cast<const u8*>(STEREO_CAMERA_SETTINGS.data()));
+    if (!res.IsSuccess())
+        return res;
+    res = CreateConfigInfoBlk(0x00090001, sizeof(CONSOLE_UNIQUE_ID), 0xE,
+                              reinterpret_cast<const u8*>(&CONSOLE_UNIQUE_ID));
+    if (!res.IsSuccess())
+        return res;
+    res = CreateConfigInfoBlk(0x000F0004, sizeof(CONSOLE_MODEL), 0x8,
+                              reinterpret_cast<const u8*>(&CONSOLE_MODEL));
+    if (!res.IsSuccess())
+        return res;
+    res = CreateConfigInfoBlk(0x000A0002, sizeof(CONSOLE_LANGUAGE), 0xA, &CONSOLE_LANGUAGE);
+    if (!res.IsSuccess())
+        return res;
+    res = CreateConfigInfoBlk(0x00070001, sizeof(SOUND_OUTPUT_MODE), 0xE, &SOUND_OUTPUT_MODE);
+    if (!res.IsSuccess())
+        return res;
+    res = CreateConfigInfoBlk(0x000B0000, sizeof(COUNTRY_INFO), 0xE,
+                              reinterpret_cast<const u8*>(&COUNTRY_INFO));
+    if (!res.IsSuccess())
+        return res;
+    res = CreateConfigInfoBlk(0x000A0000, sizeof(CONSOLE_USERNAME_BLOCK), 0xE,
+                              reinterpret_cast<const u8*>(&CONSOLE_USERNAME_BLOCK));
+    if (!res.IsSuccess())
+        return res;
+    // Save the buffer to the file
+    res = UpdateConfigNANDSavegame();
+    if (!res.IsSuccess())
+        return res;
+    return RESULT_SUCCESS;
+}
+
+void CFGInit() {
+    // TODO(Subv): In the future we should use the FS service to query this archive, 
+    // currently it is not possible because you can only have one open archive of the same type at any time
+    std::string syssavedata_directory = FileUtil::GetUserPath(D_SYSSAVEDATA_IDX);
+    cfg_system_save_data = Common::make_unique<FileSys::Archive_SystemSaveData>(
+                           syssavedata_directory, CFG_SAVE_ID);
+    if (!cfg_system_save_data->Initialize()) {
+        LOG_CRITICAL(Service_CFG, "Could not initialize SystemSaveData archive for the CFG:U service");
+        return;
+    }
+
+    // TODO(Subv): All this code should be moved to cfg:i, 
+    // it's only here because we do not currently emulate the lower level code that uses that service
+    // Try to open the file in read-only mode to check its existence
+    FileSys::Mode mode = {};
+    mode.read_flag = 1;
+    FileSys::Path path("config");
+    auto file = cfg_system_save_data->OpenFile(path, mode);
+
+    // Load the config if it already exists
+    if (file != nullptr) {
+        file->Read(0, CONFIG_SAVEFILE_SIZE, cfg_config_file_buffer.data());
+        return;
+    }
+
+    // Initialize the Username block
+    // TODO(Subv): Initialize this directly in the variable when MSVC supports char16_t string literals
+    CONSOLE_USERNAME_BLOCK.ng_word = 0;
+    CONSOLE_USERNAME_BLOCK.zero = 0;
+    // Copy string to buffer and pad with zeros at the end
+    auto size = Common::UTF8ToUTF16(CONSOLE_USERNAME).copy(CONSOLE_USERNAME_BLOCK.username, 0x14);
+    std::fill(std::begin(CONSOLE_USERNAME_BLOCK.username) + size,
+              std::end(CONSOLE_USERNAME_BLOCK.username), 0);
+    FormatConfig();
+}
+
+void CFGShutdown() {
+
+}
+
+} // namespace CFG
+} // namespace Service
diff --git a/src/core/hle/service/cfg/cfg.h b/src/core/hle/service/cfg/cfg.h
new file mode 100644
index 000000000..c74527ca4
--- /dev/null
+++ b/src/core/hle/service/cfg/cfg.h
@@ -0,0 +1,144 @@
+// Copyright 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#pragma once
+
+#include <array>
+#include "core/hle/result.h"
+
+namespace Service {
+namespace CFG {
+
+enum SystemModel {
+    NINTENDO_3DS = 0,
+    NINTENDO_3DS_XL = 1,
+    NEW_NINTENDO_3DS = 2,
+    NINTENDO_2DS = 3,
+    NEW_NINTENDO_3DS_XL = 4
+};
+
+enum SystemLanguage {
+    LANGUAGE_JP = 0,
+    LANGUAGE_EN = 1,
+    LANGUAGE_FR = 2,
+    LANGUAGE_DE = 3,
+    LANGUAGE_IT = 4,
+    LANGUAGE_ES = 5,
+    LANGUAGE_ZH = 6,
+    LANGUAGE_KO = 7,
+    LANGUAGE_NL = 8,
+    LANGUAGE_PT = 9,
+    LANGUAGE_RU = 10
+};
+
+/// Block header in the config savedata file
+struct SaveConfigBlockEntry {
+    u32 block_id;       ///< The id of the current block
+    u32 offset_or_data; ///< This is the absolute offset to the block data if the size is greater than 4 bytes, otherwise it contains the data itself
+    u16 size;           ///< The size of the block
+    u16 flags;          ///< The flags of the block, possibly used for access control
+};
+
+/// The maximum number of block entries that can exist in the config file
+static const u32 CONFIG_FILE_MAX_BLOCK_ENTRIES = 1479;
+
+/**
+* The header of the config savedata file,
+* contains information about the blocks in the file
+*/
+struct SaveFileConfig {
+    u16 total_entries;                        ///< The total number of set entries in the config file
+    u16 data_entries_offset;                  ///< The offset where the data for the blocks start, this is hardcoded to 0x455C as per hardware
+    SaveConfigBlockEntry block_entries[CONFIG_FILE_MAX_BLOCK_ENTRIES]; ///< The block headers, the maximum possible value is 1479 as per hardware
+    u32 unknown;                              ///< This field is unknown, possibly padding, 0 has been observed in hardware
+};
+static_assert(sizeof(SaveFileConfig) == 0x455C, "The SaveFileConfig header must be exactly 0x455C bytes");
+
+struct UsernameBlock {
+    char16_t username[10]; ///< Exactly 20 bytes long, padded with zeros at the end if necessary
+    u32 zero;
+    u32 ng_word;
+};
+static_assert(sizeof(UsernameBlock) == 0x1C, "Size of UsernameBlock must be 0x1C");
+
+struct ConsoleModelInfo {
+    u8 model;       ///< The console model (3DS, 2DS, etc)
+    u8 unknown[3];  ///< Unknown data
+};
+static_assert(sizeof(ConsoleModelInfo) == 4, "ConsoleModelInfo must be exactly 4 bytes");
+
+struct ConsoleCountryInfo {
+    u8 unknown[3];   ///< Unknown data
+    u8 country_code; ///< The country code of the console
+};
+static_assert(sizeof(ConsoleCountryInfo) == 4, "ConsoleCountryInfo must be exactly 4 bytes");
+
+extern const u64 CFG_SAVE_ID;
+extern const u64 CONSOLE_UNIQUE_ID;
+extern const ConsoleModelInfo CONSOLE_MODEL;
+extern const u8 CONSOLE_LANGUAGE;
+extern const char CONSOLE_USERNAME[0x14];
+/// This will be initialized in the Interface constructor, and will be used when creating the block
+extern UsernameBlock CONSOLE_USERNAME_BLOCK;
+/// TODO(Subv): Find out what this actually is
+extern const u8 SOUND_OUTPUT_MODE;
+extern const u8 UNITED_STATES_COUNTRY_ID;
+/// TODO(Subv): Find what the other bytes are
+extern const ConsoleCountryInfo COUNTRY_INFO;
+extern const std::array<float, 8> STEREO_CAMERA_SETTINGS;
+
+static_assert(sizeof(STEREO_CAMERA_SETTINGS) == 0x20, "STEREO_CAMERA_SETTINGS must be exactly 0x20 bytes");
+static_assert(sizeof(CONSOLE_UNIQUE_ID) == 8, "CONSOLE_UNIQUE_ID must be exactly 8 bytes");
+static_assert(sizeof(CONSOLE_LANGUAGE) == 1, "CONSOLE_LANGUAGE must be exactly 1 byte");
+static_assert(sizeof(SOUND_OUTPUT_MODE) == 1, "SOUND_OUTPUT_MODE must be exactly 1 byte");
+
+/**
+ * Reads a block with the specified id and flag from the Config savegame buffer
+ * and writes the output to output.
+ * The input size must match exactly the size of the requested block
+ * @param block_id The id of the block we want to read
+ * @param size The size of the block we want to read
+ * @param flag The requested block must have this flag set
+ * @param output A pointer where we will write the read data
+ * @returns ResultCode indicating the result of the operation, 0 on success
+ */
+ResultCode GetConfigInfoBlock(u32 block_id, u32 size, u32 flag, u8* output);
+
+/**
+ * Creates a block with the specified id and writes the input data to the cfg savegame buffer in memory.
+ * The config savegame file in the filesystem is not updated.
+ * @param block_id The id of the block we want to create
+ * @param size The size of the block we want to create
+ * @param flag The flags of the new block
+ * @param data A pointer containing the data we will write to the new block
+ * @returns ResultCode indicating the result of the operation, 0 on success
+ */
+ResultCode CreateConfigInfoBlk(u32 block_id, u16 size, u16 flags, const u8* data);
+
+/**
+ * Deletes the config savegame file from the filesystem, the buffer in memory is not affected
+ * @returns ResultCode indicating the result of the operation, 0 on success
+ */
+ResultCode DeleteConfigNANDSaveFile();
+
+/**
+ * Writes the config savegame memory buffer to the config savegame file in the filesystem
+ * @returns ResultCode indicating the result of the operation, 0 on success
+ */
+ResultCode UpdateConfigNANDSavegame();
+
+/**
+ * Re-creates the config savegame file in memory and the filesystem with the default blocks
+ * @returns ResultCode indicating the result of the operation, 0 on success
+ */
+ResultCode FormatConfig();
+
+/// Initialize the config service
+void CFGInit();
+
+/// Shutdown the config service
+void CFGShutdown();
+
+} // namespace CFG
+} // namespace Service
diff --git a/src/core/hle/service/cfg/cfg_i.cpp b/src/core/hle/service/cfg/cfg_i.cpp
new file mode 100644
index 000000000..7c1ee8ac3
--- /dev/null
+++ b/src/core/hle/service/cfg/cfg_i.cpp
@@ -0,0 +1,110 @@
+// Copyright 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#include "common/log.h"
+#include "core/hle/hle.h"
+#include "core/hle/service/cfg/cfg.h"
+#include "core/hle/service/cfg/cfg_i.h"
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// Namespace CFG_I
+
+namespace CFG_I {
+    
+/**
+ * CFG_I::GetConfigInfoBlk8 service function
+ * This function is called by two command headers,
+ * there appears to be no difference between them according to 3dbrew
+ *  Inputs:
+ *      0 : 0x04010082 / 0x08010082
+ *      1 : Size
+ *      2 : Block ID
+ *      3 : Descriptor for the output buffer
+ *      4 : Output buffer pointer
+ *  Outputs:
+ *      1 : Result of function, 0 on success, otherwise error code
+ */
+static void GetConfigInfoBlk8(Service::Interface* self) {
+    u32* cmd_buffer = Kernel::GetCommandBuffer();
+    u32 size = cmd_buffer[1];
+    u32 block_id = cmd_buffer[2];
+    u8* data_pointer = Memory::GetPointer(cmd_buffer[4]);
+
+    if (data_pointer == nullptr) {
+        cmd_buffer[1] = -1; // TODO(Subv): Find the right error code
+        return;
+    }
+
+    cmd_buffer[1] = Service::CFG::GetConfigInfoBlock(block_id, size, 0x8, data_pointer).raw;
+}
+
+/**
+ * CFG_I::UpdateConfigNANDSavegame service function
+ * This function is called by two command headers, 
+ * there appears to be no difference between them according to 3dbrew
+ *  Inputs:
+ *      0 : 0x04030000 / 0x08030000
+ *  Outputs:
+ *      1 : Result of function, 0 on success, otherwise error code
+ */
+static void UpdateConfigNANDSavegame(Service::Interface* self) {
+    u32* cmd_buffer = Kernel::GetCommandBuffer();
+    cmd_buffer[1] = Service::CFG::UpdateConfigNANDSavegame().raw;
+}
+
+/**
+ * CFG_I::FormatConfig service function
+ *  Inputs:
+ *      0 : 0x08060000
+ *  Outputs:
+ *      1 : Result of function, 0 on success, otherwise error code
+ */
+static void FormatConfig(Service::Interface* self) {
+    u32* cmd_buffer = Kernel::GetCommandBuffer();
+    cmd_buffer[1] = Service::CFG::FormatConfig().raw;
+}
+
+const Interface::FunctionInfo FunctionTable[] = {
+    {0x04010082, GetConfigInfoBlk8,                    "GetConfigInfoBlk8"},
+    {0x04020082, nullptr,                              "SetConfigInfoBlk4"},
+    {0x04030000, UpdateConfigNANDSavegame,             "UpdateConfigNANDSavegame"},
+    {0x04040042, nullptr,                              "GetLocalFriendCodeSeedData"},
+    {0x04050000, nullptr,                              "GetLocalFriendCodeSeed"},
+    {0x04060000, nullptr,                              "SecureInfoGetRegion"},
+    {0x04070000, nullptr,                              "SecureInfoGetByte101"},
+    {0x04080042, nullptr,                              "SecureInfoGetSerialNo"},
+    {0x04090000, nullptr,                              "UpdateConfigBlk00040003"},
+    {0x08010082, GetConfigInfoBlk8,                    "GetConfigInfoBlk8"},
+    {0x08020082, nullptr,                              "SetConfigInfoBlk4"},
+    {0x08030000, UpdateConfigNANDSavegame,             "UpdateConfigNANDSavegame"},
+    {0x080400C2, nullptr,                              "CreateConfigInfoBlk"},
+    {0x08050000, nullptr,                              "DeleteConfigNANDSavefile"},
+    {0x08060000, FormatConfig,                         "FormatConfig"},
+    {0x08080000, nullptr,                              "UpdateConfigBlk1"},
+    {0x08090000, nullptr,                              "UpdateConfigBlk2"},
+    {0x080A0000, nullptr,                              "UpdateConfigBlk3"},
+    {0x080B0082, nullptr,                              "SetGetLocalFriendCodeSeedData"},
+    {0x080C0042, nullptr,                              "SetLocalFriendCodeSeedSignature"},
+    {0x080D0000, nullptr,                              "DeleteCreateNANDLocalFriendCodeSeed"},
+    {0x080E0000, nullptr,                              "VerifySigLocalFriendCodeSeed"},
+    {0x080F0042, nullptr,                              "GetLocalFriendCodeSeedData"},
+    {0x08100000, nullptr,                              "GetLocalFriendCodeSeed"},
+    {0x08110084, nullptr,                              "SetSecureInfo"},
+    {0x08120000, nullptr,                              "DeleteCreateNANDSecureInfo"},
+    {0x08130000, nullptr,                              "VerifySigSecureInfo"},
+    {0x08140042, nullptr,                              "SecureInfoGetData"},
+    {0x08150042, nullptr,                              "SecureInfoGetSignature"},
+    {0x08160000, nullptr,                              "SecureInfoGetRegion"},
+    {0x08170000, nullptr,                              "SecureInfoGetByte101"},
+    {0x08180042, nullptr,                              "SecureInfoGetSerialNo"},
+};
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// Interface class
+
+Interface::Interface() {
+    Register(FunctionTable, ARRAY_SIZE(FunctionTable));
+}
+
+} // namespace
diff --git a/src/core/hle/service/cfg_i.h b/src/core/hle/service/cfg/cfg_i.h
index fe343c968..a498dd589 100644
--- a/src/core/hle/service/cfg_i.h
+++ b/src/core/hle/service/cfg/cfg_i.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -14,11 +14,7 @@ namespace CFG_I {
 class Interface : public Service::Interface {
 public:
     Interface();
-    ~Interface();
-    /**
-     * Gets the string port name used by CTROS for the service
-     * @return Port name of service
-     */
+
     std::string GetPortName() const override {
         return "cfg:i";
     }
diff --git a/src/core/hle/service/cfg_u.cpp b/src/core/hle/service/cfg/cfg_u.cpp
index 972cc0534..03c01cf90 100644
--- a/src/core/hle/service/cfg_u.cpp
+++ b/src/core/hle/service/cfg/cfg_u.cpp
@@ -1,10 +1,14 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
+#include "common/file_util.h"
 #include "common/log.h"
+#include "common/string_util.h"
+#include "core/file_sys/archive_systemsavedata.h"
 #include "core/hle/hle.h"
-#include "core/hle/service/cfg_u.h"
+#include "core/hle/service/cfg/cfg.h"
+#include "core/hle/service/cfg/cfg_u.h"
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////
 // Namespace CFG_U
@@ -52,7 +56,7 @@ static const std::array<u16, 187> country_codes = {
  *      2 : Country's 2-char string
  */
 static void GetCountryCodeString(Service::Interface* self) {
-    u32* cmd_buffer = Service::GetCommandBuffer();
+    u32* cmd_buffer = Kernel::GetCommandBuffer();
     u32 country_code_id = cmd_buffer[1];
 
     if (country_code_id >= country_codes.size() || 0 == country_codes[country_code_id]) {
@@ -74,7 +78,7 @@ static void GetCountryCodeString(Service::Interface* self) {
  *      2 : Country Code ID
  */
 static void GetCountryCodeID(Service::Interface* self) {
-    u32* cmd_buffer = Service::GetCommandBuffer();
+    u32* cmd_buffer = Kernel::GetCommandBuffer();
     u16 country_code = cmd_buffer[1];
     u16 country_code_id = 0;
 
@@ -99,18 +103,85 @@ static void GetCountryCodeID(Service::Interface* self) {
     cmd_buffer[2] = country_code_id;
 }
 
+/**
+ * CFG_User::GetConfigInfoBlk2 service function
+ *  Inputs:
+ *      0 : 0x00010082
+ *      1 : Size
+ *      2 : Block ID
+ *      3 : Descriptor for the output buffer
+ *      4 : Output buffer pointer
+ *  Outputs:
+ *      1 : Result of function, 0 on success, otherwise error code
+ */
+static void GetConfigInfoBlk2(Service::Interface* self) {
+    u32* cmd_buffer = Kernel::GetCommandBuffer();
+    u32 size = cmd_buffer[1];
+    u32 block_id = cmd_buffer[2];
+    u8* data_pointer = Memory::GetPointer(cmd_buffer[4]);
+    
+    if (data_pointer == nullptr) {
+        cmd_buffer[1] = -1; // TODO(Subv): Find the right error code
+        return;
+    }
+
+    cmd_buffer[1] = Service::CFG::GetConfigInfoBlock(block_id, size, 0x2, data_pointer).raw;
+}
+
+/**
+ * CFG_User::GetSystemModel service function
+ *  Inputs:
+ *      0 : 0x00050000
+ *  Outputs:
+ *      1 : Result of function, 0 on success, otherwise error code
+ *      2 : Model of the console
+ */
+static void GetSystemModel(Service::Interface* self) {
+    u32* cmd_buffer = Kernel::GetCommandBuffer();
+    u32 data;
+
+    // TODO(Subv): Find out the correct error codes
+    cmd_buffer[1] = Service::CFG::GetConfigInfoBlock(0x000F0004, 4, 0x8,
+                                                     reinterpret_cast<u8*>(&data)).raw; 
+    cmd_buffer[2] = data & 0xFF;
+}
+
+/**
+ * CFG_User::GetModelNintendo2DS service function
+ *  Inputs:
+ *      0 : 0x00060000
+ *  Outputs:
+ *      1 : Result of function, 0 on success, otherwise error code
+ *      2 : 0 if the system is a Nintendo 2DS, 1 otherwise
+ */
+static void GetModelNintendo2DS(Service::Interface* self) {
+    u32* cmd_buffer = Kernel::GetCommandBuffer();
+    u32 data;
+
+    // TODO(Subv): Find out the correct error codes
+    cmd_buffer[1] = Service::CFG::GetConfigInfoBlock(0x000F0004, 4, 0x8,
+                                                     reinterpret_cast<u8*>(&data)).raw; 
+    
+    u8 model = data & 0xFF;
+    if (model == Service::CFG::NINTENDO_2DS)
+        cmd_buffer[2] = 0;
+    else
+        cmd_buffer[2] = 1;
+}
+
 const Interface::FunctionInfo FunctionTable[] = {
-    {0x00010082, nullptr,               "GetConfigInfoBlk2"},
+    {0x00010082, GetConfigInfoBlk2,     "GetConfigInfoBlk2"},
     {0x00020000, nullptr,               "SecureInfoGetRegion"},
     {0x00030000, nullptr,               "GenHashConsoleUnique"},
     {0x00040000, nullptr,               "GetRegionCanadaUSA"},
-    {0x00050000, nullptr,               "GetSystemModel"},
-    {0x00060000, nullptr,               "GetModelNintendo2DS"},
+    {0x00050000, GetSystemModel,        "GetSystemModel"},
+    {0x00060000, GetModelNintendo2DS,   "GetModelNintendo2DS"},
     {0x00070040, nullptr,               "unknown"},
     {0x00080080, nullptr,               "unknown"},
     {0x00090040, GetCountryCodeString,  "GetCountryCodeString"},
     {0x000A0040, GetCountryCodeID,      "GetCountryCodeID"},
 };
+
 ////////////////////////////////////////////////////////////////////////////////////////////////////
 // Interface class
 
@@ -118,7 +189,4 @@ Interface::Interface() {
     Register(FunctionTable, ARRAY_SIZE(FunctionTable));
 }
 
-Interface::~Interface() {
-}
-
 } // namespace
diff --git a/src/core/hle/service/cfg_u.h b/src/core/hle/service/cfg/cfg_u.h
index 8075d19a8..9ad73f355 100644
--- a/src/core/hle/service/cfg_u.h
+++ b/src/core/hle/service/cfg/cfg_u.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -14,11 +14,7 @@ namespace CFG_U {
 class Interface : public Service::Interface {
 public:
     Interface();
-    ~Interface();
-    /**
-     * Gets the string port name used by CTROS for the service
-     * @return Port name of service
-     */
+
     std::string GetPortName() const override {
         return "cfg:u";
     }
diff --git a/src/core/hle/service/cfg_i.cpp b/src/core/hle/service/cfg_i.cpp
deleted file mode 100644
index 88d13d459..000000000
--- a/src/core/hle/service/cfg_i.cpp
+++ /dev/null
@@ -1,59 +0,0 @@
-// Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
-// Refer to the license.txt file included.
-
-#include "common/log.h"
-#include "core/hle/hle.h"
-#include "core/hle/service/cfg_i.h"
-
-////////////////////////////////////////////////////////////////////////////////////////////////////
-// Namespace CFG_I
-
-namespace CFG_I {
-
-const Interface::FunctionInfo FunctionTable[] = {
-    {0x04010082, nullptr,               "GetConfigInfoBlk8"},
-    {0x04020082, nullptr,               "GetConfigInfoBlk4"},
-    {0x04030000, nullptr,               "UpdateConfigNANDSavegame"},
-    {0x04040042, nullptr,               "GetLocalFriendCodeSeedData"},
-    {0x04050000, nullptr,               "GetLocalFriendCodeSeed"},
-    {0x04060000, nullptr,               "SecureInfoGetRegion"},
-    {0x04070000, nullptr,               "SecureInfoGetByte101"},
-    {0x04080042, nullptr,               "SecureInfoGetSerialNo"},
-    {0x04090000, nullptr,               "UpdateConfigBlk00040003"},
-    {0x08010082, nullptr,               "GetConfigInfoBlk8"},
-    {0x08020082, nullptr,               "GetConfigInfoBlk4"},
-    {0x08030000, nullptr,               "UpdateConfigNANDSavegame"},
-    {0x080400C2, nullptr,               "CreateConfigInfoBlk"},
-    {0x08050000, nullptr,               "DeleteConfigNANDSavefile"},
-    {0x08060000, nullptr,               "FormatConfig"},
-    {0x08070000, nullptr,               "Unknown"},
-    {0x08080000, nullptr,               "UpdateConfigBlk1"},
-    {0x08090000, nullptr,               "UpdateConfigBlk2"},
-    {0x080A0000, nullptr,               "UpdateConfigBlk3"},
-    {0x080B0082, nullptr,               "SetGetLocalFriendCodeSeedData"},
-    {0x080C0042, nullptr,               "SetLocalFriendCodeSeedSignature"},
-    {0x080D0000, nullptr,               "DeleteCreateNANDLocalFriendCodeSeed"},
-    {0x080E0000, nullptr,               "VerifySigLocalFriendCodeSeed"},
-    {0x080F0042, nullptr,               "GetLocalFriendCodeSeedData"},
-    {0x08100000, nullptr,               "GetLocalFriendCodeSeed"},
-    {0x08110084, nullptr,               "SetSecureInfo"},
-    {0x08120000, nullptr,               "DeleteCreateNANDSecureInfo"},
-    {0x08130000, nullptr,               "VerifySigSecureInfo"},
-    {0x08140042, nullptr,               "SecureInfoGetData"},
-    {0x08150042, nullptr,               "SecureInfoGetSignature"},
-    {0x08160000, nullptr,               "SecureInfoGetRegion"},
-    {0x08170000, nullptr,               "SecureInfoGetByte101"},
-    {0x08180042, nullptr,               "SecureInfoGetSerialNo"},
-};
-////////////////////////////////////////////////////////////////////////////////////////////////////
-// Interface class
-
-Interface::Interface() {
-    Register(FunctionTable, ARRAY_SIZE(FunctionTable));
-}
-
-Interface::~Interface() {
-}
-
-} // namespace
diff --git a/src/core/hle/service/csnd_snd.cpp b/src/core/hle/service/csnd_snd.cpp
index 6e59a9bf3..aef8cfbca 100644
--- a/src/core/hle/service/csnd_snd.cpp
+++ b/src/core/hle/service/csnd_snd.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "common/log.h"
@@ -33,7 +33,4 @@ Interface::Interface() {
     Register(FunctionTable, ARRAY_SIZE(FunctionTable));
 }
 
-Interface::~Interface() {
-}
-
 } // namespace
diff --git a/src/core/hle/service/csnd_snd.h b/src/core/hle/service/csnd_snd.h
index 31cc85b07..a84752473 100644
--- a/src/core/hle/service/csnd_snd.h
+++ b/src/core/hle/service/csnd_snd.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -14,11 +14,7 @@ namespace CSND_SND {
 class Interface : public Service::Interface {
 public:
     Interface();
-    ~Interface();
-    /**
-     * Gets the string port name used by CTROS for the service
-     * @return Port name of service
-     */
+
     std::string GetPortName() const override {
         return "csnd:SND";
     }
diff --git a/src/core/hle/service/dsp_dsp.cpp b/src/core/hle/service/dsp_dsp.cpp
index ce1c9938d..2cf4d118f 100644
--- a/src/core/hle/service/dsp_dsp.cpp
+++ b/src/core/hle/service/dsp_dsp.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "common/log.h"
@@ -25,7 +25,7 @@ static Handle interrupt_event;
  *      2 : (inaddr << 1) + 0x1FF40000 (where 0x1FF00000 is the DSP RAM address)
  */
 void ConvertProcessAddressFromDspDram(Service::Interface* self) {
-    u32* cmd_buff = Service::GetCommandBuffer();
+    u32* cmd_buff = Kernel::GetCommandBuffer();
 
     u32 addr = cmd_buff[1];
 
@@ -48,7 +48,7 @@ void ConvertProcessAddressFromDspDram(Service::Interface* self) {
  *      2 : Component loaded, 0 on not loaded, 1 on loaded
  */
 void LoadComponent(Service::Interface* self) {
-    u32* cmd_buff = Service::GetCommandBuffer();
+    u32* cmd_buff = Kernel::GetCommandBuffer();
 
     cmd_buff[1] = 0; // No error
     cmd_buff[2] = 1; // Pretend that we actually loaded the DSP firmware
@@ -65,7 +65,7 @@ void LoadComponent(Service::Interface* self) {
  *      3 : Semaphore event handle
  */
 void GetSemaphoreEventHandle(Service::Interface* self) {
-    u32* cmd_buff = Service::GetCommandBuffer();
+    u32* cmd_buff = Kernel::GetCommandBuffer();
 
     cmd_buff[1] = 0; // No error
     cmd_buff[3] = semaphore_event; // Event handle
@@ -83,7 +83,7 @@ void GetSemaphoreEventHandle(Service::Interface* self) {
  *      1 : Result of function, 0 on success, otherwise error code
  */
 void RegisterInterruptEvents(Service::Interface* self) {
-    u32* cmd_buff = Service::GetCommandBuffer();
+    u32* cmd_buff = Kernel::GetCommandBuffer();
 
     interrupt_event = static_cast<Handle>(cmd_buff[4]);
 
@@ -100,7 +100,7 @@ void RegisterInterruptEvents(Service::Interface* self) {
  *      1 : Result of function, 0 on success, otherwise error code
  */
 void WriteReg0x10(Service::Interface* self) {
-    u32* cmd_buff = Service::GetCommandBuffer();
+    u32* cmd_buff = Kernel::GetCommandBuffer();
 
     Kernel::SignalEvent(interrupt_event);
 
@@ -121,7 +121,7 @@ void WriteReg0x10(Service::Interface* self) {
  *      2 : Number of bytes read from pipe
  */
 void ReadPipeIfPossible(Service::Interface* self) {
-    u32* cmd_buff = Service::GetCommandBuffer();
+    u32* cmd_buff = Kernel::GetCommandBuffer();
 
     u32 size = cmd_buff[3] & 0xFFFF;// Lower 16 bits are size
     VAddr addr = cmd_buff[0x41];
@@ -190,7 +190,4 @@ Interface::Interface() {
     Register(FunctionTable, ARRAY_SIZE(FunctionTable));
 }
 
-Interface::~Interface() {
-}
-
 } // namespace
diff --git a/src/core/hle/service/dsp_dsp.h b/src/core/hle/service/dsp_dsp.h
index 9431b62f6..0b8b64600 100644
--- a/src/core/hle/service/dsp_dsp.h
+++ b/src/core/hle/service/dsp_dsp.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -14,11 +14,7 @@ namespace DSP_DSP {
 class Interface : public Service::Interface {
 public:
     Interface();
-    ~Interface();
-    /**
-     * Gets the string port name used by CTROS for the service
-     * @return Port name of service
-     */
+
     std::string GetPortName() const override {
         return "dsp::DSP";
     }
diff --git a/src/core/hle/service/err_f.cpp b/src/core/hle/service/err_f.cpp
index 785c351e9..8c900eabc 100644
--- a/src/core/hle/service/err_f.cpp
+++ b/src/core/hle/service/err_f.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "common/log.h"
@@ -11,17 +11,15 @@
 
 namespace ERR_F {
 
-    const Interface::FunctionInfo FunctionTable[] = {
-        {0x00010800, nullptr,               "ThrowFatalError"}
-    };
-    ////////////////////////////////////////////////////////////////////////////////////////////////////
-    // Interface class
+const Interface::FunctionInfo FunctionTable[] = {
+    {0x00010800, nullptr,               "ThrowFatalError"}
+};
 
-    Interface::Interface() {
-        Register(FunctionTable, ARRAY_SIZE(FunctionTable));
-    }
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// Interface class
 
-    Interface::~Interface() {
-    }
+Interface::Interface() {
+    Register(FunctionTable, ARRAY_SIZE(FunctionTable));
+}
 
 } // namespace
diff --git a/src/core/hle/service/err_f.h b/src/core/hle/service/err_f.h
index 6d7141c1b..892d8af9b 100644
--- a/src/core/hle/service/err_f.h
+++ b/src/core/hle/service/err_f.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -11,17 +11,13 @@
 
 namespace ERR_F {
 
-    class Interface : public Service::Interface {
-    public:
-        Interface();
-        ~Interface();
-        /**
-         * Gets the string port name used by CTROS for the service
-         * @return Port name of service
-         */
-        std::string GetPortName() const override {
-            return "err:f";
-        }
-    };
+class Interface : public Service::Interface {
+public:
+    Interface();
+
+    std::string GetPortName() const override {
+        return "err:f";
+    }
+};
 
 } // namespace
diff --git a/src/core/hle/service/frd_u.cpp b/src/core/hle/service/frd_u.cpp
index 58023e536..021186e57 100644
--- a/src/core/hle/service/frd_u.cpp
+++ b/src/core/hle/service/frd_u.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "common/log.h"
@@ -11,25 +11,23 @@
 
 namespace FRD_U {
 
-    const Interface::FunctionInfo FunctionTable[] = {
-        {0x00050000, nullptr,               "GetFriendKey"},
-        {0x00080000, nullptr,               "GetMyPresence"},
-        {0x00100040, nullptr,               "GetPassword"},
-        {0x00190042, nullptr,               "GetFriendFavoriteGame"},
-        {0x001A00C4, nullptr,               "GetFriendInfo"},
-        {0x001B0080, nullptr,               "IsOnFriendList"},
-        {0x001C0042, nullptr,               "DecodeLocalFriendCode"},
-        {0x001D0002, nullptr,               "SetCurrentlyPlayingText"},
-        {0x00320042, nullptr,               "SetClientSdkVersion"}
-    };
-    ////////////////////////////////////////////////////////////////////////////////////////////////////
-    // Interface class
+const Interface::FunctionInfo FunctionTable[] = {
+    {0x00050000, nullptr,               "GetFriendKey"},
+    {0x00080000, nullptr,               "GetMyPresence"},
+    {0x00100040, nullptr,               "GetPassword"},
+    {0x00190042, nullptr,               "GetFriendFavoriteGame"},
+    {0x001A00C4, nullptr,               "GetFriendInfo"},
+    {0x001B0080, nullptr,               "IsOnFriendList"},
+    {0x001C0042, nullptr,               "DecodeLocalFriendCode"},
+    {0x001D0002, nullptr,               "SetCurrentlyPlayingText"},
+    {0x00320042, nullptr,               "SetClientSdkVersion"}
+};
 
-    Interface::Interface() {
-        Register(FunctionTable, ARRAY_SIZE(FunctionTable));
-    }
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// Interface class
 
-    Interface::~Interface() {
-    }
+Interface::Interface() {
+    Register(FunctionTable, ARRAY_SIZE(FunctionTable));
+}
 
 } // namespace
diff --git a/src/core/hle/service/frd_u.h b/src/core/hle/service/frd_u.h
index 4020c6664..ab8897d5b 100644
--- a/src/core/hle/service/frd_u.h
+++ b/src/core/hle/service/frd_u.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -11,17 +11,13 @@
 
 namespace FRD_U {
 
-    class Interface : public Service::Interface {
-    public:
-        Interface();
-        ~Interface();
-        /**
-         * Gets the string port name used by CTROS for the service
-         * @return Port name of service
-         */
-        std::string GetPortName() const override {
-            return "frd:u";
-        }
-    };
+class Interface : public Service::Interface {
+public:
+    Interface();
+
+    std::string GetPortName() const override {
+        return "frd:u";
+    }
+};
 
 } // namespace
diff --git a/src/core/hle/service/fs/archive.cpp b/src/core/hle/service/fs/archive.cpp
new file mode 100644
index 000000000..487bf3aa7
--- /dev/null
+++ b/src/core/hle/service/fs/archive.cpp
@@ -0,0 +1,442 @@
+// Copyright 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#include <memory>
+#include <unordered_map>
+
+#include "common/common_types.h"
+#include "common/file_util.h"
+#include "common/make_unique.h"
+#include "common/math_util.h"
+
+#include "core/file_sys/archive_savedata.h"
+#include "core/file_sys/archive_backend.h"
+#include "core/file_sys/archive_sdmc.h"
+#include "core/file_sys/directory_backend.h"
+#include "core/hle/service/fs/archive.h"
+#include "core/hle/kernel/session.h"
+#include "core/hle/result.h"
+
+// Specializes std::hash for ArchiveIdCode, so that we can use it in std::unordered_map.
+// Workaroung for libstdc++ bug: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=60970
+namespace std {
+    template <>
+    struct hash<Service::FS::ArchiveIdCode> {
+        typedef Service::FS::ArchiveIdCode argument_type;
+        typedef std::size_t result_type;
+
+        result_type operator()(const argument_type& id_code) const {
+            typedef std::underlying_type<argument_type>::type Type;
+            return std::hash<Type>()(static_cast<Type>(id_code));
+        }
+    };
+}
+
+namespace Service {
+namespace FS {
+
+// Command to access archive file
+enum class FileCommand : u32 {
+    Dummy1          = 0x000100C6,
+    Control         = 0x040100C4,
+    OpenSubFile     = 0x08010100,
+    Read            = 0x080200C2,
+    Write           = 0x08030102,
+    GetSize         = 0x08040000,
+    SetSize         = 0x08050080,
+    GetAttributes   = 0x08060000,
+    SetAttributes   = 0x08070040,
+    Close           = 0x08080000,
+    Flush           = 0x08090000,
+};
+
+// Command to access directory
+enum class DirectoryCommand : u32 {
+    Dummy1          = 0x000100C6,
+    Control         = 0x040100C4,
+    Read            = 0x08010042,
+    Close           = 0x08020000,
+};
+
+class Archive {
+public:
+    Archive(std::unique_ptr<FileSys::ArchiveBackend>&& backend, ArchiveIdCode id_code)
+            : backend(std::move(backend)), id_code(id_code) {
+    }
+
+    std::string GetName() const { return "Archive: " + backend->GetName(); }
+
+    ArchiveIdCode id_code; ///< Id code of the archive
+    std::unique_ptr<FileSys::ArchiveBackend> backend; ///< Archive backend interface
+};
+
+class File : public Kernel::Session {
+public:
+    File(std::unique_ptr<FileSys::FileBackend>&& backend, const FileSys::Path& path)
+            : backend(std::move(backend)), path(path) {
+    }
+
+    std::string GetName() const override { return "Path: " + path.DebugStr(); }
+
+    FileSys::Path path; ///< Path of the file
+    std::unique_ptr<FileSys::FileBackend> backend; ///< File backend interface
+
+    ResultVal<bool> SyncRequest() override {
+        u32* cmd_buff = Kernel::GetCommandBuffer();
+        FileCommand cmd = static_cast<FileCommand>(cmd_buff[0]);
+        switch (cmd) {
+
+        // Read from file...
+        case FileCommand::Read:
+        {
+            u64 offset = cmd_buff[1] | ((u64) cmd_buff[2]) << 32;
+            u32 length  = cmd_buff[3];
+            u32 address = cmd_buff[5];
+            LOG_TRACE(Service_FS, "Read %s %s: offset=0x%llx length=%d address=0x%x",
+                      GetTypeName().c_str(), GetName().c_str(), offset, length, address);
+            cmd_buff[2] = backend->Read(offset, length, Memory::GetPointer(address));
+            break;
+        }
+
+        // Write to file...
+        case FileCommand::Write:
+        {
+            u64 offset  = cmd_buff[1] | ((u64) cmd_buff[2]) << 32;
+            u32 length  = cmd_buff[3];
+            u32 flush   = cmd_buff[4];
+            u32 address = cmd_buff[6];
+            LOG_TRACE(Service_FS, "Write %s %s: offset=0x%llx length=%d address=0x%x, flush=0x%x",
+                      GetTypeName().c_str(), GetName().c_str(), offset, length, address, flush);
+            cmd_buff[2] = backend->Write(offset, length, flush, Memory::GetPointer(address));
+            break;
+        }
+
+        case FileCommand::GetSize:
+        {
+            LOG_TRACE(Service_FS, "GetSize %s %s", GetTypeName().c_str(), GetName().c_str());
+            u64 size = backend->GetSize();
+            cmd_buff[2] = (u32)size;
+            cmd_buff[3] = size >> 32;
+            break;
+        }
+
+        case FileCommand::SetSize:
+        {
+            u64 size = cmd_buff[1] | ((u64)cmd_buff[2] << 32);
+            LOG_TRACE(Service_FS, "SetSize %s %s size=%llu",
+                    GetTypeName().c_str(), GetName().c_str(), size);
+            backend->SetSize(size);
+            break;
+        }
+
+        case FileCommand::Close:
+        {
+            LOG_TRACE(Service_FS, "Close %s %s", GetTypeName().c_str(), GetName().c_str());
+            backend->Close();
+            break;
+        }
+
+        case FileCommand::Flush:
+        {
+            LOG_TRACE(Service_FS, "Flush");
+            backend->Flush();
+            break;
+        }
+
+        // Unknown command...
+        default:
+            LOG_ERROR(Service_FS, "Unknown command=0x%08X!", cmd);
+            ResultCode error = UnimplementedFunction(ErrorModule::FS);
+            cmd_buff[1] = error.raw; // TODO(Link Mauve): use the correct error code for that.
+            return error;
+        }
+        cmd_buff[1] = 0; // No error
+        return MakeResult<bool>(false);
+    }
+};
+
+class Directory : public Kernel::Session {
+public:
+    Directory(std::unique_ptr<FileSys::DirectoryBackend>&& backend, const FileSys::Path& path)
+            : backend(std::move(backend)), path(path) {
+    }
+
+    std::string GetName() const override { return "Directory: " + path.DebugStr(); }
+
+    FileSys::Path path; ///< Path of the directory
+    std::unique_ptr<FileSys::DirectoryBackend> backend; ///< File backend interface
+
+    ResultVal<bool> SyncRequest() override {
+        u32* cmd_buff = Kernel::GetCommandBuffer();
+        DirectoryCommand cmd = static_cast<DirectoryCommand>(cmd_buff[0]);
+        switch (cmd) {
+
+        // Read from directory...
+        case DirectoryCommand::Read:
+        {
+            u32 count = cmd_buff[1];
+            u32 address = cmd_buff[3];
+            auto entries = reinterpret_cast<FileSys::Entry*>(Memory::GetPointer(address));
+            LOG_TRACE(Service_FS, "Read %s %s: count=%d",
+                    GetTypeName().c_str(), GetName().c_str(), count);
+
+            // Number of entries actually read
+            cmd_buff[2] = backend->Read(count, entries);
+            break;
+        }
+
+        case DirectoryCommand::Close:
+        {
+            LOG_TRACE(Service_FS, "Close %s %s", GetTypeName().c_str(), GetName().c_str());
+            backend->Close();
+            break;
+        }
+
+        // Unknown command...
+        default:
+            LOG_ERROR(Service_FS, "Unknown command=0x%08X!", cmd);
+            ResultCode error = UnimplementedFunction(ErrorModule::FS);
+            cmd_buff[1] = error.raw; // TODO(Link Mauve): use the correct error code for that.
+            return MakeResult<bool>(false);
+        }
+        cmd_buff[1] = 0; // No error
+        return MakeResult<bool>(false);
+    }
+};
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+
+/**
+ * Map of registered archives, identified by id code. Once an archive is registered here, it is
+ * never removed until the FS service is shut down.
+ */
+static std::unordered_map<ArchiveIdCode, std::unique_ptr<Archive>> id_code_map;
+
+/**
+ * Map of active archive handles. Values are pointers to the archives in `idcode_map`.
+ */
+static std::unordered_map<ArchiveHandle, Archive*> handle_map;
+static ArchiveHandle next_handle;
+
+static Archive* GetArchive(ArchiveHandle handle) {
+    auto itr = handle_map.find(handle);
+    return (itr == handle_map.end()) ? nullptr : itr->second;
+}
+
+ResultVal<ArchiveHandle> OpenArchive(ArchiveIdCode id_code) {
+    LOG_TRACE(Service_FS, "Opening archive with id code 0x%08X", id_code);
+
+    auto itr = id_code_map.find(id_code);
+    if (itr == id_code_map.end()) {
+        if (id_code == ArchiveIdCode::SaveData) {
+            // When a SaveData archive is created for the first time, it is not yet formatted
+            // and the save file/directory structure expected by the game has not yet been initialized. 
+            // Returning the NotFormatted error code will signal the game to provision the SaveData archive 
+            // with the files and folders that it expects. 
+            // The FormatSaveData service call will create the SaveData archive when it is called.
+            return ResultCode(ErrorDescription::FS_NotFormatted, ErrorModule::FS,
+                              ErrorSummary::InvalidState, ErrorLevel::Status);
+        }
+        // TODO: Verify error against hardware
+        return ResultCode(ErrorDescription::NotFound, ErrorModule::FS,
+                          ErrorSummary::NotFound, ErrorLevel::Permanent);
+    }
+
+    // This should never even happen in the first place with 64-bit handles, 
+    while (handle_map.count(next_handle) != 0) {
+        ++next_handle;
+    }
+    handle_map.emplace(next_handle, itr->second.get());
+    return MakeResult<ArchiveHandle>(next_handle++);
+}
+
+ResultCode CloseArchive(ArchiveHandle handle) {
+    if (handle_map.erase(handle) == 0)
+        return InvalidHandle(ErrorModule::FS);
+    else
+        return RESULT_SUCCESS;
+}
+
+// TODO(yuriks): This might be what the fs:REG service is for. See the Register/Unregister calls in
+// http://3dbrew.org/wiki/Filesystem_services#ProgramRegistry_service_.22fs:REG.22
+ResultCode CreateArchive(std::unique_ptr<FileSys::ArchiveBackend>&& backend, ArchiveIdCode id_code) {
+    auto result = id_code_map.emplace(id_code, Common::make_unique<Archive>(std::move(backend), id_code));
+
+    bool inserted = result.second;
+    _dbg_assert_msg_(Service_FS, inserted, "Tried to register more than one archive with same id code");
+
+    auto& archive = result.first->second;
+    LOG_DEBUG(Service_FS, "Registered archive %s with id code 0x%08X", archive->GetName().c_str(), id_code);
+    return RESULT_SUCCESS;
+}
+
+ResultVal<Handle> OpenFileFromArchive(ArchiveHandle archive_handle, const FileSys::Path& path, const FileSys::Mode mode) {
+    Archive* archive = GetArchive(archive_handle);
+    if (archive == nullptr)
+        return InvalidHandle(ErrorModule::FS);
+
+    std::unique_ptr<FileSys::FileBackend> backend = archive->backend->OpenFile(path, mode);
+    if (backend == nullptr) {
+        return ResultCode(ErrorDescription::FS_NotFound, ErrorModule::FS,
+                          ErrorSummary::NotFound, ErrorLevel::Status);
+    }
+
+    auto file = Common::make_unique<File>(std::move(backend), path);
+    // TOOD(yuriks): Fix error reporting
+    Handle handle = Kernel::g_handle_table.Create(file.release()).ValueOr(INVALID_HANDLE);
+    return MakeResult<Handle>(handle);
+}
+
+ResultCode DeleteFileFromArchive(ArchiveHandle archive_handle, const FileSys::Path& path) {
+    Archive* archive = GetArchive(archive_handle);
+    if (archive == nullptr)
+        return InvalidHandle(ErrorModule::FS);
+
+    if (archive->backend->DeleteFile(path))
+        return RESULT_SUCCESS;
+    return ResultCode(ErrorDescription::NoData, ErrorModule::FS, // TODO: verify description
+                      ErrorSummary::Canceled, ErrorLevel::Status);
+}
+
+ResultCode RenameFileBetweenArchives(ArchiveHandle src_archive_handle, const FileSys::Path& src_path,
+                                     ArchiveHandle dest_archive_handle, const FileSys::Path& dest_path) {
+    Archive* src_archive = GetArchive(src_archive_handle);
+    Archive* dest_archive = GetArchive(dest_archive_handle);
+    if (src_archive == nullptr || dest_archive == nullptr)
+        return InvalidHandle(ErrorModule::FS);
+
+    if (src_archive == dest_archive) {
+        if (src_archive->backend->RenameFile(src_path, dest_path))
+            return RESULT_SUCCESS;
+    } else {
+        // TODO: Implement renaming across archives
+        return UnimplementedFunction(ErrorModule::FS);
+    }
+
+    // TODO(yuriks): This code probably isn't right, it'll return a Status even if the file didn't
+    // exist or similar. Verify.
+    return ResultCode(ErrorDescription::NoData, ErrorModule::FS, // TODO: verify description
+                      ErrorSummary::NothingHappened, ErrorLevel::Status);
+}
+
+ResultCode DeleteDirectoryFromArchive(ArchiveHandle archive_handle, const FileSys::Path& path) {
+    Archive* archive = GetArchive(archive_handle);
+    if (archive == nullptr)
+        return InvalidHandle(ErrorModule::FS);
+
+    if (archive->backend->DeleteDirectory(path))
+        return RESULT_SUCCESS;
+    return ResultCode(ErrorDescription::NoData, ErrorModule::FS, // TODO: verify description
+                      ErrorSummary::Canceled, ErrorLevel::Status);
+}
+
+ResultCode CreateFileInArchive(Handle archive_handle, const FileSys::Path& path, u32 file_size) {
+    Archive* archive = GetArchive(archive_handle);
+    if (archive == nullptr)
+        return InvalidHandle(ErrorModule::FS);
+
+    return archive->backend->CreateFile(path, file_size);
+}
+
+ResultCode CreateDirectoryFromArchive(ArchiveHandle archive_handle, const FileSys::Path& path) {
+    Archive* archive = GetArchive(archive_handle);
+    if (archive == nullptr)
+        return InvalidHandle(ErrorModule::FS);
+
+    if (archive->backend->CreateDirectory(path))
+        return RESULT_SUCCESS;
+    return ResultCode(ErrorDescription::NoData, ErrorModule::FS, // TODO: verify description
+                      ErrorSummary::Canceled, ErrorLevel::Status);
+}
+
+ResultCode RenameDirectoryBetweenArchives(ArchiveHandle src_archive_handle, const FileSys::Path& src_path,
+                                          ArchiveHandle dest_archive_handle, const FileSys::Path& dest_path) {
+    Archive* src_archive = GetArchive(src_archive_handle);
+    Archive* dest_archive = GetArchive(dest_archive_handle);
+    if (src_archive == nullptr || dest_archive == nullptr)
+        return InvalidHandle(ErrorModule::FS);
+
+    if (src_archive == dest_archive) {
+        if (src_archive->backend->RenameDirectory(src_path, dest_path))
+            return RESULT_SUCCESS;
+    } else {
+        // TODO: Implement renaming across archives
+        return UnimplementedFunction(ErrorModule::FS);
+    }
+
+    // TODO(yuriks): This code probably isn't right, it'll return a Status even if the file didn't
+    // exist or similar. Verify.
+    return ResultCode(ErrorDescription::NoData, ErrorModule::FS, // TODO: verify description
+                      ErrorSummary::NothingHappened, ErrorLevel::Status);
+}
+
+/**
+ * Open a Directory from an Archive
+ * @param archive_handle Handle to an open Archive object
+ * @param path Path to the Directory inside of the Archive
+ * @return Opened Directory object
+ */
+ResultVal<Handle> OpenDirectoryFromArchive(ArchiveHandle archive_handle, const FileSys::Path& path) {
+    Archive* archive = GetArchive(archive_handle);
+    if (archive == nullptr)
+        return InvalidHandle(ErrorModule::FS);
+
+    std::unique_ptr<FileSys::DirectoryBackend> backend = archive->backend->OpenDirectory(path);
+    if (backend == nullptr) {
+        return ResultCode(ErrorDescription::NotFound, ErrorModule::FS,
+                          ErrorSummary::NotFound, ErrorLevel::Permanent);
+    }
+
+    auto directory = Common::make_unique<Directory>(std::move(backend), path);
+    // TOOD(yuriks): Fix error reporting
+    Handle handle = Kernel::g_handle_table.Create(directory.release()).ValueOr(INVALID_HANDLE);
+    return MakeResult<Handle>(handle);
+}
+
+ResultCode FormatSaveData() {
+    // TODO(Subv): Actually wipe the savedata folder after creating or opening it
+
+    // Do not create the archive again if it already exists
+    if (id_code_map.find(ArchiveIdCode::SaveData) != id_code_map.end())
+        return UnimplementedFunction(ErrorModule::FS); // TODO(Subv): Find the correct error code
+
+    // Create the SaveData archive
+    std::string savedata_directory = FileUtil::GetUserPath(D_SAVEDATA_IDX);
+    auto savedata_archive = Common::make_unique<FileSys::Archive_SaveData>(savedata_directory,
+        Kernel::g_program_id);
+
+    if (savedata_archive->Initialize()) {
+        CreateArchive(std::move(savedata_archive), ArchiveIdCode::SaveData);
+        return RESULT_SUCCESS;
+    } else {
+        LOG_ERROR(Service_FS, "Can't instantiate SaveData archive with path %s",
+            savedata_archive->GetMountPoint().c_str());
+        return UnimplementedFunction(ErrorModule::FS); // TODO(Subv): Find the proper error code
+    }
+}
+
+/// Initialize archives
+void ArchiveInit() {
+    next_handle = 1;
+
+    // TODO(Link Mauve): Add the other archive types (see here for the known types:
+    // http://3dbrew.org/wiki/FS:OpenArchive#Archive_idcodes).  Currently the only half-finished
+    // archive type is SDMC, so it is the only one getting exposed.
+
+    std::string sdmc_directory = FileUtil::GetUserPath(D_SDMC_IDX);
+    auto sdmc_archive = Common::make_unique<FileSys::Archive_SDMC>(sdmc_directory);
+    if (sdmc_archive->Initialize())
+        CreateArchive(std::move(sdmc_archive), ArchiveIdCode::SDMC);
+    else
+        LOG_ERROR(Service_FS, "Can't instantiate SDMC archive with path %s", sdmc_directory.c_str());
+}
+
+/// Shutdown archives
+void ArchiveShutdown() {
+    handle_map.clear();
+    id_code_map.clear();
+}
+
+} // namespace FS
+} // namespace Service
diff --git a/src/core/hle/kernel/archive.h b/src/core/hle/service/fs/archive.h
index b50833a2b..b39bc41b6 100644
--- a/src/core/hle/kernel/archive.h
+++ b/src/core/hle/service/fs/archive.h
@@ -1,39 +1,50 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
 
 #include "common/common_types.h"
 
-#include "core/file_sys/archive.h"
+#include "core/file_sys/archive_backend.h"
 #include "core/hle/kernel/kernel.h"
 #include "core/hle/result.h"
 
-////////////////////////////////////////////////////////////////////////////////////////////////////
-// Kernel namespace
+namespace Service {
+namespace FS {
 
-namespace Kernel {
+/// Supported archive types
+enum class ArchiveIdCode : u32 {
+    RomFS               = 0x00000003,
+    SaveData            = 0x00000004,
+    ExtSaveData         = 0x00000006,
+    SharedExtSaveData   = 0x00000007,
+    SystemSaveData      = 0x00000008,
+    SDMC                = 0x00000009,
+    SDMCWriteOnly       = 0x0000000A,
+};
+
+typedef u64 ArchiveHandle;
 
 /**
  * Opens an archive
  * @param id_code IdCode of the archive to open
  * @return Handle to the opened archive
  */
-ResultVal<Handle> OpenArchive(FileSys::Archive::IdCode id_code);
+ResultVal<ArchiveHandle> OpenArchive(ArchiveIdCode id_code);
 
 /**
  * Closes an archive
  * @param id_code IdCode of the archive to open
  */
-ResultCode CloseArchive(FileSys::Archive::IdCode id_code);
+ResultCode CloseArchive(ArchiveHandle handle);
 
 /**
  * Creates an Archive
  * @param backend File system backend interface to the archive
- * @param name Name of Archive
+ * @param id_code Id code used to access this type of archive
  */
-ResultCode CreateArchive(FileSys::Archive* backend, const std::string& name);
+ResultCode CreateArchive(std::unique_ptr<FileSys::ArchiveBackend>&& backend, ArchiveIdCode id_code);
 
 /**
  * Open a File from an Archive
@@ -42,7 +53,7 @@ ResultCode CreateArchive(FileSys::Archive* backend, const std::string& name);
  * @param mode Mode under which to open the File
  * @return Handle to the opened File object
  */
-ResultVal<Handle> OpenFileFromArchive(Handle archive_handle, const FileSys::Path& path, const FileSys::Mode mode);
+ResultVal<Handle> OpenFileFromArchive(ArchiveHandle archive_handle, const FileSys::Path& path, const FileSys::Mode mode);
 
 /**
  * Delete a File from an Archive
@@ -50,7 +61,7 @@ ResultVal<Handle> OpenFileFromArchive(Handle archive_handle, const FileSys::Path
  * @param path Path to the File inside of the Archive
  * @return Whether deletion succeeded
  */
-ResultCode DeleteFileFromArchive(Handle archive_handle, const FileSys::Path& path);
+ResultCode DeleteFileFromArchive(ArchiveHandle archive_handle, const FileSys::Path& path);
 
 /**
  * Rename a File between two Archives
@@ -60,8 +71,8 @@ ResultCode DeleteFileFromArchive(Handle archive_handle, const FileSys::Path& pat
  * @param dest_path Path to the File inside of the destination Archive
  * @return Whether rename succeeded
  */
-ResultCode RenameFileBetweenArchives(Handle src_archive_handle, const FileSys::Path& src_path,
-                                     Handle dest_archive_handle, const FileSys::Path& dest_path);
+ResultCode RenameFileBetweenArchives(ArchiveHandle src_archive_handle, const FileSys::Path& src_path,
+                                     ArchiveHandle dest_archive_handle, const FileSys::Path& dest_path);
 
 /**
  * Delete a Directory from an Archive
@@ -69,7 +80,16 @@ ResultCode RenameFileBetweenArchives(Handle src_archive_handle, const FileSys::P
  * @param path Path to the Directory inside of the Archive
  * @return Whether deletion succeeded
  */
-ResultCode DeleteDirectoryFromArchive(Handle archive_handle, const FileSys::Path& path);
+ResultCode DeleteDirectoryFromArchive(ArchiveHandle archive_handle, const FileSys::Path& path);
+
+/**
+ * Create a File in an Archive
+ * @param archive_handle Handle to an open Archive object
+ * @param path Path to the File inside of the Archive
+ * @param file_size The size of the new file, filled with zeroes
+ * @return File creation result code
+ */
+ResultCode CreateFileInArchive(Handle archive_handle, const FileSys::Path& path, u32 file_size);
 
 /**
  * Create a Directory from an Archive
@@ -77,7 +97,7 @@ ResultCode DeleteDirectoryFromArchive(Handle archive_handle, const FileSys::Path
  * @param path Path to the Directory inside of the Archive
  * @return Whether creation of directory succeeded
  */
-ResultCode CreateDirectoryFromArchive(Handle archive_handle, const FileSys::Path& path);
+ResultCode CreateDirectoryFromArchive(ArchiveHandle archive_handle, const FileSys::Path& path);
 
 /**
  * Rename a Directory between two Archives
@@ -87,8 +107,8 @@ ResultCode CreateDirectoryFromArchive(Handle archive_handle, const FileSys::Path
  * @param dest_path Path to the Directory inside of the destination Archive
  * @return Whether rename succeeded
  */
-ResultCode RenameDirectoryBetweenArchives(Handle src_archive_handle, const FileSys::Path& src_path,
-                                          Handle dest_archive_handle, const FileSys::Path& dest_path);
+ResultCode RenameDirectoryBetweenArchives(ArchiveHandle src_archive_handle, const FileSys::Path& src_path,
+                                          ArchiveHandle dest_archive_handle, const FileSys::Path& dest_path);
 
 /**
  * Open a Directory from an Archive
@@ -96,7 +116,13 @@ ResultCode RenameDirectoryBetweenArchives(Handle src_archive_handle, const FileS
  * @param path Path to the Directory inside of the Archive
  * @return Handle to the opened File object
  */
-ResultVal<Handle> OpenDirectoryFromArchive(Handle archive_handle, const FileSys::Path& path);
+ResultVal<Handle> OpenDirectoryFromArchive(ArchiveHandle archive_handle, const FileSys::Path& path);
+
+/**
+ * Creates a blank SaveData archive.
+ * @return ResultCode 0 on success or the corresponding code on error
+ */
+ResultCode FormatSaveData();
 
 /// Initialize archives
 void ArchiveInit();
@@ -104,4 +130,5 @@ void ArchiveInit();
 /// Shutdown archives
 void ArchiveShutdown();
 
-} // namespace FileSys
+} // namespace FS
+} // namespace Service
diff --git a/src/core/hle/service/fs_user.cpp b/src/core/hle/service/fs/fs_user.cpp
index 9bda4fe8a..b1a465274 100644
--- a/src/core/hle/service/fs_user.cpp
+++ b/src/core/hle/service/fs/fs_user.cpp
@@ -1,23 +1,28 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "common/common.h"
-
+#include "common/file_util.h"
+#include "common/scope_exit.h"
 #include "common/string_util.h"
-#include "core/hle/kernel/archive.h"
-#include "core/hle/kernel/archive.h"
 #include "core/hle/result.h"
-#include "core/hle/service/fs_user.h"
+#include "core/hle/service/fs/archive.h"
+#include "core/hle/service/fs/fs_user.h"
 #include "core/settings.h"
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////
 // Namespace FS_User
 
-namespace FS_User {
+namespace Service {
+namespace FS {
+
+static ArchiveHandle MakeArchiveHandle(u32 low_word, u32 high_word) {
+    return (u64)low_word | ((u64)high_word << 32);
+}
 
 static void Initialize(Service::Interface* self) {
-    u32* cmd_buff = Service::GetCommandBuffer();
+    u32* cmd_buff = Kernel::GetCommandBuffer();
 
     // TODO(Link Mauve): check the behavior when cmd_buff[1] isn't 32, as per
     // http://3dbrew.org/wiki/FS:Initialize#Request
@@ -43,11 +48,9 @@ static void Initialize(Service::Interface* self) {
  *      3 : File handle
  */
 static void OpenFile(Service::Interface* self) {
-    u32* cmd_buff = Service::GetCommandBuffer();
+    u32* cmd_buff = Kernel::GetCommandBuffer();
 
-    // TODO(Link Mauve): cmd_buff[2], aka archive handle lower word, isn't used according to
-    // 3dmoo's or ctrulib's implementations.  Triple check if it's really the case.
-    Handle archive_handle = static_cast<Handle>(cmd_buff[3]);
+    ArchiveHandle archive_handle = MakeArchiveHandle(cmd_buff[2], cmd_buff[3]);
     auto filename_type    = static_cast<FileSys::LowPathType>(cmd_buff[4]);
     u32 filename_size     = cmd_buff[5];
     FileSys::Mode mode; mode.hex = cmd_buff[6];
@@ -57,11 +60,12 @@ static void OpenFile(Service::Interface* self) {
 
     LOG_DEBUG(Service_FS, "path=%s, mode=%d attrs=%u", file_path.DebugStr().c_str(), mode.hex, attributes);
 
-    ResultVal<Handle> handle = Kernel::OpenFileFromArchive(archive_handle, file_path, mode);
+    ResultVal<Handle> handle = OpenFileFromArchive(archive_handle, file_path, mode);
     cmd_buff[1] = handle.Code().raw;
     if (handle.Succeeded()) {
         cmd_buff[3] = *handle;
     } else {
+        cmd_buff[3] = 0;
         LOG_ERROR(Service_FS, "failed to get a handle for file %s", file_path.DebugStr().c_str());
     }
 }
@@ -86,9 +90,9 @@ static void OpenFile(Service::Interface* self) {
  *      3 : File handle
  */
 static void OpenFileDirectly(Service::Interface* self) {
-    u32* cmd_buff = Service::GetCommandBuffer();
+    u32* cmd_buff = Kernel::GetCommandBuffer();
 
-    auto archive_id       = static_cast<FileSys::Archive::IdCode>(cmd_buff[2]);
+    auto archive_id       = static_cast<FS::ArchiveIdCode>(cmd_buff[2]);
     auto archivename_type = static_cast<FileSys::LowPathType>(cmd_buff[3]);
     u32 archivename_size  = cmd_buff[4];
     auto filename_type    = static_cast<FileSys::LowPathType>(cmd_buff[5]);
@@ -106,25 +110,25 @@ static void OpenFileDirectly(Service::Interface* self) {
     if (archive_path.GetType() != FileSys::Empty) {
         LOG_ERROR(Service_FS, "archive LowPath type other than empty is currently unsupported");
         cmd_buff[1] = UnimplementedFunction(ErrorModule::FS).raw;
+        cmd_buff[3] = 0;
         return;
     }
 
-    // TODO(Link Mauve): Check if we should even get a handle for the archive, and don't leak it
-    // TODO(yuriks): Why is there all this duplicate (and seemingly useless) code up here?
-    ResultVal<Handle> archive_handle = Kernel::OpenArchive(archive_id);
-    cmd_buff[1] = archive_handle.Code().raw;
+    ResultVal<ArchiveHandle> archive_handle = OpenArchive(archive_id);
     if (archive_handle.Failed()) {
         LOG_ERROR(Service_FS, "failed to get a handle for archive");
+        cmd_buff[1] = archive_handle.Code().raw;
+        cmd_buff[3] = 0;
         return;
     }
-    // cmd_buff[2] isn't used according to 3dmoo's implementation.
-    cmd_buff[3] = *archive_handle;
+    SCOPE_EXIT({ CloseArchive(*archive_handle); });
 
-    ResultVal<Handle> handle = Kernel::OpenFileFromArchive(*archive_handle, file_path, mode);
+    ResultVal<Handle> handle = OpenFileFromArchive(*archive_handle, file_path, mode);
     cmd_buff[1] = handle.Code().raw;
     if (handle.Succeeded()) {
         cmd_buff[3] = *handle;
     } else {
+        cmd_buff[3] = 0;
         LOG_ERROR(Service_FS, "failed to get a handle for file %s", file_path.DebugStr().c_str());
     }
 }
@@ -140,12 +144,10 @@ static void OpenFileDirectly(Service::Interface* self) {
  *  Outputs:
  *      1 : Result of function, 0 on success, otherwise error code
  */
-void DeleteFile(Service::Interface* self) {
-    u32* cmd_buff = Service::GetCommandBuffer();
+static void DeleteFile(Service::Interface* self) {
+    u32* cmd_buff = Kernel::GetCommandBuffer();
 
-    // TODO(Link Mauve): cmd_buff[2], aka archive handle lower word, isn't used according to
-    // 3dmoo's or ctrulib's implementations.  Triple check if it's really the case.
-    Handle archive_handle = static_cast<Handle>(cmd_buff[3]);
+    ArchiveHandle archive_handle = MakeArchiveHandle(cmd_buff[2], cmd_buff[3]);
     auto filename_type    = static_cast<FileSys::LowPathType>(cmd_buff[4]);
     u32 filename_size     = cmd_buff[5];
     u32 filename_ptr      = cmd_buff[7];
@@ -155,7 +157,7 @@ void DeleteFile(Service::Interface* self) {
     LOG_DEBUG(Service_FS, "type=%d size=%d data=%s",
               filename_type, filename_size, file_path.DebugStr().c_str());
 
-    cmd_buff[1] = Kernel::DeleteFileFromArchive(archive_handle, file_path).raw;
+    cmd_buff[1] = DeleteFileFromArchive(archive_handle, file_path).raw;
 }
 
 /*
@@ -174,15 +176,13 @@ void DeleteFile(Service::Interface* self) {
  *  Outputs:
  *      1 : Result of function, 0 on success, otherwise error code
  */
-void RenameFile(Service::Interface* self) {
-    u32* cmd_buff = Service::GetCommandBuffer();
+static void RenameFile(Service::Interface* self) {
+    u32* cmd_buff = Kernel::GetCommandBuffer();
 
-    // TODO(Link Mauve): cmd_buff[2] and cmd_buff[6], aka archive handle lower word, aren't used according to
-    // 3dmoo's or ctrulib's implementations.  Triple check if it's really the case.
-    Handle src_archive_handle  = static_cast<Handle>(cmd_buff[3]);
+    ArchiveHandle src_archive_handle = MakeArchiveHandle(cmd_buff[2], cmd_buff[3]);
     auto src_filename_type     = static_cast<FileSys::LowPathType>(cmd_buff[4]);
     u32 src_filename_size      = cmd_buff[5];
-    Handle dest_archive_handle = static_cast<Handle>(cmd_buff[7]);
+    ArchiveHandle dest_archive_handle = MakeArchiveHandle(cmd_buff[6], cmd_buff[7]);;
     auto dest_filename_type    = static_cast<FileSys::LowPathType>(cmd_buff[8]);
     u32 dest_filename_size     = cmd_buff[9];
     u32 src_filename_ptr       = cmd_buff[11];
@@ -195,7 +195,7 @@ void RenameFile(Service::Interface* self) {
               src_filename_type, src_filename_size, src_file_path.DebugStr().c_str(),
               dest_filename_type, dest_filename_size, dest_file_path.DebugStr().c_str());
 
-    cmd_buff[1] = Kernel::RenameFileBetweenArchives(src_archive_handle, src_file_path, dest_archive_handle, dest_file_path).raw;
+    cmd_buff[1] = RenameFileBetweenArchives(src_archive_handle, src_file_path, dest_archive_handle, dest_file_path).raw;
 }
 
 /*
@@ -209,12 +209,10 @@ void RenameFile(Service::Interface* self) {
  *  Outputs:
  *      1 : Result of function, 0 on success, otherwise error code
  */
-void DeleteDirectory(Service::Interface* self) {
-    u32* cmd_buff = Service::GetCommandBuffer();
+static void DeleteDirectory(Service::Interface* self) {
+    u32* cmd_buff = Kernel::GetCommandBuffer();
 
-    // TODO(Link Mauve): cmd_buff[2], aka archive handle lower word, isn't used according to
-    // 3dmoo's or ctrulib's implementations.  Triple check if it's really the case.
-    Handle archive_handle = static_cast<Handle>(cmd_buff[3]);
+    ArchiveHandle archive_handle = MakeArchiveHandle(cmd_buff[2], cmd_buff[3]);
     auto dirname_type     = static_cast<FileSys::LowPathType>(cmd_buff[4]);
     u32 dirname_size      = cmd_buff[5];
     u32 dirname_ptr       = cmd_buff[7];
@@ -224,7 +222,36 @@ void DeleteDirectory(Service::Interface* self) {
     LOG_DEBUG(Service_FS, "type=%d size=%d data=%s",
               dirname_type, dirname_size, dir_path.DebugStr().c_str());
     
-    cmd_buff[1] = Kernel::DeleteDirectoryFromArchive(archive_handle, dir_path).raw;
+    cmd_buff[1] = DeleteDirectoryFromArchive(archive_handle, dir_path).raw;
+}
+
+/*
+ * FS_User::CreateFile service function
+ *  Inputs:
+ *      0 : Command header 0x08080202
+ *      2 : Archive handle lower word
+ *      3 : Archive handle upper word
+ *      4 : File path string type
+ *      5 : File path string size
+ *      7 : File size (filled with zeroes)
+ *      10: File path string data
+ *  Outputs:
+ *      1 : Result of function, 0 on success, otherwise error code
+ */
+static void CreateFile(Service::Interface* self) {
+    u32* cmd_buff = Kernel::GetCommandBuffer();
+
+    ArchiveHandle archive_handle = MakeArchiveHandle(cmd_buff[2], cmd_buff[3]);
+    auto filename_type    = static_cast<FileSys::LowPathType>(cmd_buff[4]);
+    u32 filename_size     = cmd_buff[5];
+    u32 file_size         = cmd_buff[7];
+    u32 filename_ptr      = cmd_buff[10];
+
+    FileSys::Path file_path(filename_type, filename_size, filename_ptr);
+
+    LOG_DEBUG(Service_FS, "type=%d size=%d data=%s", filename_type, filename_size, file_path.DebugStr().c_str());
+
+    cmd_buff[1] = CreateFileInArchive(archive_handle, file_path, file_size).raw;
 }
 
 /*
@@ -239,11 +266,9 @@ void DeleteDirectory(Service::Interface* self) {
  *      1 : Result of function, 0 on success, otherwise error code
  */
 static void CreateDirectory(Service::Interface* self) {
-    u32* cmd_buff = Service::GetCommandBuffer();
+    u32* cmd_buff = Kernel::GetCommandBuffer();
 
-    // TODO: cmd_buff[2], aka archive handle lower word, isn't used according to
-    // 3dmoo's or ctrulib's implementations.  Triple check if it's really the case.
-    Handle archive_handle = static_cast<Handle>(cmd_buff[3]);
+    ArchiveHandle archive_handle = MakeArchiveHandle(cmd_buff[2], cmd_buff[3]);
     auto dirname_type = static_cast<FileSys::LowPathType>(cmd_buff[4]);
     u32 dirname_size = cmd_buff[5];
     u32 dirname_ptr = cmd_buff[8];
@@ -252,7 +277,7 @@ static void CreateDirectory(Service::Interface* self) {
 
     LOG_DEBUG(Service_FS, "type=%d size=%d data=%s", dirname_type, dirname_size, dir_path.DebugStr().c_str());
 
-    cmd_buff[1] = Kernel::CreateDirectoryFromArchive(archive_handle, dir_path).raw;
+    cmd_buff[1] = CreateDirectoryFromArchive(archive_handle, dir_path).raw;
 }
 
 /*
@@ -271,15 +296,13 @@ static void CreateDirectory(Service::Interface* self) {
  *  Outputs:
  *      1 : Result of function, 0 on success, otherwise error code
  */
-void RenameDirectory(Service::Interface* self) {
-    u32* cmd_buff = Service::GetCommandBuffer();
+static void RenameDirectory(Service::Interface* self) {
+    u32* cmd_buff = Kernel::GetCommandBuffer();
 
-    // TODO(Link Mauve): cmd_buff[2] and cmd_buff[6], aka archive handle lower word, aren't used according to
-    // 3dmoo's or ctrulib's implementations.  Triple check if it's really the case.
-    Handle src_archive_handle  = static_cast<Handle>(cmd_buff[3]);
+    ArchiveHandle src_archive_handle = MakeArchiveHandle(cmd_buff[2], cmd_buff[3]);
     auto src_dirname_type      = static_cast<FileSys::LowPathType>(cmd_buff[4]);
     u32 src_dirname_size       = cmd_buff[5];
-    Handle dest_archive_handle = static_cast<Handle>(cmd_buff[7]);
+    ArchiveHandle dest_archive_handle = MakeArchiveHandle(cmd_buff[6], cmd_buff[7]);
     auto dest_dirname_type     = static_cast<FileSys::LowPathType>(cmd_buff[8]);
     u32 dest_dirname_size      = cmd_buff[9];
     u32 src_dirname_ptr        = cmd_buff[11];
@@ -292,15 +315,26 @@ void RenameDirectory(Service::Interface* self) {
               src_dirname_type, src_dirname_size, src_dir_path.DebugStr().c_str(),
               dest_dirname_type, dest_dirname_size, dest_dir_path.DebugStr().c_str());
 
-    cmd_buff[1] = Kernel::RenameDirectoryBetweenArchives(src_archive_handle, src_dir_path, dest_archive_handle, dest_dir_path).raw;
+    cmd_buff[1] = RenameDirectoryBetweenArchives(src_archive_handle, src_dir_path, dest_archive_handle, dest_dir_path).raw;
 }
 
+/**
+ * FS_User::OpenDirectory service function
+ *  Inputs:
+ *      1 : Archive handle low word
+ *      2 : Archive handle high word
+ *      3 : Low path type
+ *      4 : Low path size
+ *      7 : (LowPathSize << 14) | 2
+ *      8 : Low path data pointer
+ *  Outputs:
+ *      1 : Result of function, 0 on success, otherwise error code
+ *      3 : Directory handle
+ */
 static void OpenDirectory(Service::Interface* self) {
-    u32* cmd_buff = Service::GetCommandBuffer();
+    u32* cmd_buff = Kernel::GetCommandBuffer();
 
-    // TODO(Link Mauve): cmd_buff[2], aka archive handle lower word, isn't used according to
-    // 3dmoo's or ctrulib's implementations.  Triple check if it's really the case.
-    Handle archive_handle = static_cast<Handle>(cmd_buff[2]);
+    ArchiveHandle archive_handle = MakeArchiveHandle(cmd_buff[1], cmd_buff[2]);
     auto dirname_type = static_cast<FileSys::LowPathType>(cmd_buff[3]);
     u32 dirname_size = cmd_buff[4];
     u32 dirname_ptr = cmd_buff[6];
@@ -309,7 +343,7 @@ static void OpenDirectory(Service::Interface* self) {
 
     LOG_DEBUG(Service_FS, "type=%d size=%d data=%s", dirname_type, dirname_size, dir_path.DebugStr().c_str());
 
-    ResultVal<Handle> handle = Kernel::OpenDirectoryFromArchive(archive_handle, dir_path);
+    ResultVal<Handle> handle = OpenDirectoryFromArchive(archive_handle, dir_path);
     cmd_buff[1] = handle.Code().raw;
     if (handle.Succeeded()) {
         cmd_buff[3] = *handle;
@@ -332,9 +366,9 @@ static void OpenDirectory(Service::Interface* self) {
  *      3 : Archive handle upper word (same as file handle)
  */
 static void OpenArchive(Service::Interface* self) {
-    u32* cmd_buff = Service::GetCommandBuffer();
+    u32* cmd_buff = Kernel::GetCommandBuffer();
 
-    auto archive_id       = static_cast<FileSys::Archive::IdCode>(cmd_buff[1]);
+    auto archive_id       = static_cast<FS::ArchiveIdCode>(cmd_buff[1]);
     auto archivename_type = static_cast<FileSys::LowPathType>(cmd_buff[2]);
     u32 archivename_size  = cmd_buff[3];
     u32 archivename_ptr   = cmd_buff[5];
@@ -348,16 +382,34 @@ static void OpenArchive(Service::Interface* self) {
         return;
     }
 
-    ResultVal<Handle> handle = Kernel::OpenArchive(archive_id);
+    ResultVal<ArchiveHandle> handle = OpenArchive(archive_id);
     cmd_buff[1] = handle.Code().raw;
     if (handle.Succeeded()) {
-        // cmd_buff[2] isn't used according to 3dmoo's implementation.
-        cmd_buff[3] = *handle;
+        cmd_buff[2] = *handle & 0xFFFFFFFF;
+        cmd_buff[3] = (*handle >> 32) & 0xFFFFFFFF;
     } else {
+        cmd_buff[2] = cmd_buff[3] = 0;
         LOG_ERROR(Service_FS, "failed to get a handle for archive");
     }
 }
 
+/**
+ * FS_User::CloseArchive service function
+ *  Inputs:
+ *      0 : 0x080E0080
+ *      1 : Archive handle low word
+ *      2 : Archive handle high word
+ *  Outputs:
+ *      0 : ??? TODO(yuriks): Verify return header
+ *      1 : Result of function, 0 on success, otherwise error code
+ */
+static void CloseArchive(Service::Interface* self) {
+    u32* cmd_buff = Kernel::GetCommandBuffer();
+
+    ArchiveHandle archive_handle = MakeArchiveHandle(cmd_buff[1], cmd_buff[2]);
+    cmd_buff[1] = CloseArchive(archive_handle).raw;
+}
+
 /*
 * FS_User::IsSdmcDetected service function
 *  Outputs:
@@ -365,7 +417,7 @@ static void OpenArchive(Service::Interface* self) {
 *      2 : Whether the Sdmc could be detected
 */
 static void IsSdmcDetected(Service::Interface* self) {
-    u32* cmd_buff = Service::GetCommandBuffer();
+    u32* cmd_buff = Kernel::GetCommandBuffer();
 
     cmd_buff[1] = 0;
     cmd_buff[2] = Settings::values.use_virtual_sd ? 1 : 0;
@@ -373,7 +425,83 @@ static void IsSdmcDetected(Service::Interface* self) {
     LOG_DEBUG(Service_FS, "called");
 }
 
-const Interface::FunctionInfo FunctionTable[] = {
+/**
+ * FS_User::IsSdmcWriteable service function
+ *  Outputs:
+ *      0 : Command header 0x08180000
+ *      1 : Result of function, 0 on success, otherwise error code
+ *      2 : Whether the Sdmc is currently writeable
+ */
+static void IsSdmcWriteable(Service::Interface* self) {
+    u32* cmd_buff = Kernel::GetCommandBuffer();
+
+    cmd_buff[1] = RESULT_SUCCESS.raw;
+    // If the SD isn't enabled, it can't be writeable...else, stubbed true
+    cmd_buff[2] = Settings::values.use_virtual_sd ? 1 : 0;
+    
+    LOG_DEBUG(Service_FS, " (STUBBED)");
+}
+
+/**
+ * FS_User::FormatSaveData service function, 
+ * formats the SaveData specified by the input path.
+ *  Inputs:
+ *      0  : 0x084C0242
+ *      1  : Archive ID
+ *      2  : Archive low path type
+ *      3  : Archive low path size
+ *      10 : (LowPathSize << 14) | 2
+ *      11 : Archive low path
+ *  Outputs:
+ *      1 : Result of function, 0 on success, otherwise error code
+ */
+static void FormatSaveData(Service::Interface* self) {
+    // TODO(Subv): Find out what the other inputs and outputs of this function are
+    u32* cmd_buff = Kernel::GetCommandBuffer();
+    LOG_DEBUG(Service_FS, "(STUBBED)");
+
+    auto archive_id = static_cast<FS::ArchiveIdCode>(cmd_buff[1]);
+    auto archivename_type = static_cast<FileSys::LowPathType>(cmd_buff[2]);
+    u32 archivename_size = cmd_buff[3];
+    u32 archivename_ptr = cmd_buff[11];
+    FileSys::Path archive_path(archivename_type, archivename_size, archivename_ptr);
+
+    LOG_DEBUG(Service_FS, "archive_path=%s", archive_path.DebugStr().c_str());
+
+    if (archive_id != FS::ArchiveIdCode::SaveData) {
+        // TODO(Subv): What should happen if somebody attempts to format a different archive?
+        LOG_ERROR(Service_FS, "tried to format an archive different than SaveData, %u", cmd_buff[1]);
+        cmd_buff[1] = UnimplementedFunction(ErrorModule::FS).raw;
+        return;
+    }
+
+    if (archive_path.GetType() != FileSys::LowPathType::Empty) {
+        // TODO(Subv): Implement formatting the SaveData of other games
+        LOG_ERROR(Service_FS, "archive LowPath type other than empty is currently unsupported");
+        cmd_buff[1] = UnimplementedFunction(ErrorModule::FS).raw;
+        return;
+    }
+
+    cmd_buff[1] = FormatSaveData().raw;
+}
+
+/**
+ * FS_User::FormatThisUserSaveData service function
+ *  Inputs:
+ *      0: 0x080F0180
+ *  Outputs:
+ *      1 : Result of function, 0 on success, otherwise error code
+ */
+static void FormatThisUserSaveData(Service::Interface* self) {
+    u32* cmd_buff = Kernel::GetCommandBuffer();
+    LOG_DEBUG(Service_FS, "(STUBBED)");
+
+    // TODO(Subv): Find out what the inputs and outputs of this function are
+
+    cmd_buff[1] = FormatSaveData().raw;
+}
+
+const FSUserInterface::FunctionInfo FunctionTable[] = {
     {0x000100C6, nullptr,               "Dummy1"},
     {0x040100C4, nullptr,               "Control"},
     {0x08010002, Initialize,            "Initialize"},
@@ -383,14 +511,14 @@ const Interface::FunctionInfo FunctionTable[] = {
     {0x08050244, RenameFile,            "RenameFile"},
     {0x08060142, DeleteDirectory,       "DeleteDirectory"},
     {0x08070142, nullptr,               "DeleteDirectoryRecursively"},
-    {0x08080202, nullptr,               "CreateFile"},
+    {0x08080202, CreateFile,            "CreateFile"},
     {0x08090182, CreateDirectory,       "CreateDirectory"},
     {0x080A0244, RenameDirectory,       "RenameDirectory"},
     {0x080B0102, OpenDirectory,         "OpenDirectory"},
     {0x080C00C2, OpenArchive,           "OpenArchive"},
     {0x080D0144, nullptr,               "ControlArchive"},
-    {0x080E0080, nullptr,               "CloseArchive"},
-    {0x080F0180, nullptr,               "FormatThisUserSaveData"},
+    {0x080E0080, CloseArchive,          "CloseArchive"},
+    {0x080F0180, FormatThisUserSaveData,"FormatThisUserSaveData"},
     {0x08100200, nullptr,               "CreateSystemSaveData"},
     {0x08110040, nullptr,               "DeleteSystemSaveData"},
     {0x08120080, nullptr,               "GetFreeBytes"},
@@ -399,7 +527,7 @@ const Interface::FunctionInfo FunctionTable[] = {
     {0x08150000, nullptr,               "GetNandArchiveResource"},
     {0x08160000, nullptr,               "GetSdmcFatfsErro"},
     {0x08170000, IsSdmcDetected,        "IsSdmcDetected"},
-    {0x08180000, nullptr,               "IsSdmcWritable"},
+    {0x08180000, IsSdmcWriteable,       "IsSdmcWritable"},
     {0x08190042, nullptr,               "GetSdmcCid"},
     {0x081A0042, nullptr,               "GetNandCid"},
     {0x081B0000, nullptr,               "GetSdmcSpeedInfo"},
@@ -451,7 +579,7 @@ const Interface::FunctionInfo FunctionTable[] = {
     {0x08490040, nullptr,               "GetArchiveResource"},
     {0x084A0002, nullptr,               "ExportIntegrityVerificationSeed"},
     {0x084B0002, nullptr,               "ImportIntegrityVerificationSeed"},
-    {0x084C0242, nullptr,               "FormatSaveData"},
+    {0x084C0242, FormatSaveData,        "FormatSaveData"},
     {0x084D0102, nullptr,               "GetLegacySubBannerData"},
     {0x084E0342, nullptr,               "UpdateSha256Context"},
     {0x084F0102, nullptr,               "ReadSpecialFile"},
@@ -465,11 +593,9 @@ const Interface::FunctionInfo FunctionTable[] = {
 ////////////////////////////////////////////////////////////////////////////////////////////////////
 // Interface class
 
-Interface::Interface() {
+FSUserInterface::FSUserInterface() {
     Register(FunctionTable, ARRAY_SIZE(FunctionTable));
 }
 
-Interface::~Interface() {
-}
-
-} // namespace
+} // namespace FS
+} // namespace Service
diff --git a/src/core/hle/service/fs_user.h b/src/core/hle/service/fs/fs_user.h
index 005382540..2d896dd5f 100644
--- a/src/core/hle/service/fs_user.h
+++ b/src/core/hle/service/fs/fs_user.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -9,23 +9,18 @@
 ////////////////////////////////////////////////////////////////////////////////////////////////////
 // Namespace FS_User
 
-namespace FS_User {
+namespace Service {
+namespace FS {
 
 /// Interface to "fs:USER" service
-class Interface : public Service::Interface {
+class FSUserInterface : public Service::Interface {
 public:
+    FSUserInterface();
 
-    Interface();
-
-    ~Interface();
-
-    /**
-     * Gets the string port name used by CTROS for the service
-     * @return Port name of service
-     */
     std::string GetPortName() const override {
         return "fs:USER";
     }
 };
 
-} // namespace
+} // namespace FS
+} // namespace Service
diff --git a/src/core/hle/service/gsp_gpu.cpp b/src/core/hle/service/gsp_gpu.cpp
index 223800560..0127d4ee5 100644
--- a/src/core/hle/service/gsp_gpu.cpp
+++ b/src/core/hle/service/gsp_gpu.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 
@@ -72,7 +72,7 @@ static void WriteHWRegs(u32 base_address, u32 size_in_bytes, const u32* data) {
 
 /// Write a GSP GPU hardware register
 static void WriteHWRegs(Service::Interface* self) {
-    u32* cmd_buff = Service::GetCommandBuffer();
+    u32* cmd_buff = Kernel::GetCommandBuffer();
     u32 reg_addr = cmd_buff[1];
     u32 size = cmd_buff[2];
 
@@ -83,7 +83,7 @@ static void WriteHWRegs(Service::Interface* self) {
 
 /// Read a GSP GPU hardware register
 static void ReadHWRegs(Service::Interface* self) {
-    u32* cmd_buff = Service::GetCommandBuffer();
+    u32* cmd_buff = Kernel::GetCommandBuffer();
     u32 reg_addr = cmd_buff[1];
     u32 size = cmd_buff[2];
 
@@ -136,7 +136,7 @@ static void SetBufferSwap(u32 screen_id, const FrameBufferInfo& info) {
  *      1: Result code
  */
 static void SetBufferSwap(Service::Interface* self) {
-    u32* cmd_buff = Service::GetCommandBuffer();
+    u32* cmd_buff = Kernel::GetCommandBuffer();
     u32 screen_id = cmd_buff[1];
     FrameBufferInfo* fb_info = (FrameBufferInfo*)&cmd_buff[2];
     SetBufferSwap(screen_id, *fb_info);
@@ -145,6 +145,30 @@ static void SetBufferSwap(Service::Interface* self) {
 }
 
 /**
+ * GSP_GPU::FlushDataCache service function
+ *
+ * This Function is a no-op, We aren't emulating the CPU cache any time soon.
+ *
+ *  Inputs:
+ *      1 : Address
+ *      2 : Size
+ *      3 : Value 0, some descriptor for the KProcess Handle
+ *      4 : KProcess handle
+ *  Outputs:
+ *      1 : Result of function, 0 on success, otherwise error code
+ */
+static void FlushDataCache(Service::Interface* self) {
+    u32* cmd_buff = Kernel::GetCommandBuffer();
+    u32 address = cmd_buff[1];
+    u32 size    = cmd_buff[2];
+    u32 process = cmd_buff[4];
+
+    // TODO(purpasmart96): Verify return header on HW
+
+    cmd_buff[1] = RESULT_SUCCESS.raw; // No error
+}
+
+/**
  * GSP_GPU::RegisterInterruptRelayQueue service function
  *  Inputs:
  *      1 : "Flags" field, purpose is unknown
@@ -155,7 +179,7 @@ static void SetBufferSwap(Service::Interface* self) {
  *      4 : Handle to GSP shared memory
  */
 static void RegisterInterruptRelayQueue(Service::Interface* self) {
-    u32* cmd_buff = Service::GetCommandBuffer();
+    u32* cmd_buff = Kernel::GetCommandBuffer();
     u32 flags = cmd_buff[1];
     g_interrupt_event = cmd_buff[3];
     g_shared_memory = Kernel::CreateSharedMemory("GSPSharedMem");
@@ -323,7 +347,7 @@ static void TriggerCmdReqQueue(Service::Interface* self) {
         }
     }
 
-    u32* cmd_buff = Service::GetCommandBuffer();
+    u32* cmd_buff = Kernel::GetCommandBuffer();
     cmd_buff[1] = 0; // No error
 }
 
@@ -335,7 +359,7 @@ const Interface::FunctionInfo FunctionTable[] = {
     {0x00050200, SetBufferSwap,                 "SetBufferSwap"},
     {0x00060082, nullptr,                       "SetCommandList"},
     {0x000700C2, nullptr,                       "RequestDma"},
-    {0x00080082, nullptr,                       "FlushDataCache"},
+    {0x00080082, FlushDataCache,                "FlushDataCache"},
     {0x00090082, nullptr,                       "InvalidateDataCache"},
     {0x000A0044, nullptr,                       "RegisterInterruptEvents"},
     {0x000B0040, nullptr,                       "SetLcdForceBlack"},
@@ -372,7 +396,4 @@ Interface::Interface() {
     g_thread_id = 1;
 }
 
-Interface::~Interface() {
-}
-
 } // namespace
diff --git a/src/core/hle/service/gsp_gpu.h b/src/core/hle/service/gsp_gpu.h
index 177ce8da6..932b6170f 100644
--- a/src/core/hle/service/gsp_gpu.h
+++ b/src/core/hle/service/gsp_gpu.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -158,19 +158,11 @@ static_assert(sizeof(CommandBuffer) == 0x200, "CommandBuffer struct has incorrec
 /// Interface to "srv:" service
 class Interface : public Service::Interface {
 public:
-
     Interface();
 
-    ~Interface();
-
-    /**
-     * Gets the string port name used by CTROS for the service
-     * @return Port name of service
-     */
     std::string GetPortName() const override {
         return "gsp::Gpu";
     }
-
 };
 
 /**
diff --git a/src/core/hle/service/hid_user.cpp b/src/core/hle/service/hid_user.cpp
index 5772199d4..99b0ea5a0 100644
--- a/src/core/hle/service/hid_user.cpp
+++ b/src/core/hle/service/hid_user.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "common/log.h"
@@ -153,7 +153,7 @@ void PadUpdateComplete() {
  *      8 : Event signaled by HID_User
  */
 static void GetIPCHandles(Service::Interface* self) {
-    u32* cmd_buff = Service::GetCommandBuffer();
+    u32* cmd_buff = Kernel::GetCommandBuffer();
 
     cmd_buff[1] = 0; // No error
     cmd_buff[3] = shared_mem;
@@ -179,7 +179,6 @@ const Interface::FunctionInfo FunctionTable[] = {
     {0x00170000, nullptr,       "GetSoundVolume"},
 };
 
-
 ////////////////////////////////////////////////////////////////////////////////////////////////////
 // Interface class
 
@@ -196,7 +195,4 @@ Interface::Interface() {
     Register(FunctionTable, ARRAY_SIZE(FunctionTable));
 }
 
-Interface::~Interface() {
-}
-
 } // namespace
diff --git a/src/core/hle/service/hid_user.h b/src/core/hle/service/hid_user.h
index 8f53befdb..5b96dda60 100644
--- a/src/core/hle/service/hid_user.h
+++ b/src/core/hle/service/hid_user.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -102,19 +102,11 @@ void PadUpdateComplete();
  */
 class Interface : public Service::Interface {
 public:
-
     Interface();
 
-    ~Interface();
-
-    /**
-     * Gets the string port name used by CTROS for the service
-     * @return Port name of service
-     */
     std::string GetPortName() const override {
         return "hid:USER";
     }
-
 };
 
 } // namespace
diff --git a/src/core/hle/service/http_c.cpp b/src/core/hle/service/http_c.cpp
new file mode 100644
index 000000000..d0bff552f
--- /dev/null
+++ b/src/core/hle/service/http_c.cpp
@@ -0,0 +1,64 @@
+// Copyright 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#include "common/log.h"
+#include "core/hle/hle.h"
+#include "core/hle/service/http_c.h"
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// Namespace HTTP_C
+
+namespace HTTP_C {
+
+const Interface::FunctionInfo FunctionTable[] = {
+    {0x00010044, nullptr,                 "Initialize"},
+    {0x00020082, nullptr,                 "CreateContext"},
+    {0x00030040, nullptr,                 "CloseContext"},
+    {0x00040040, nullptr,                 "CancelConnection"},
+    {0x00050040, nullptr,                 "GetRequestState"},
+    {0x00060040, nullptr,                 "GetDownloadSizeState"},
+    {0x00070040, nullptr,                 "GetRequestError"},
+    {0x00080042, nullptr,                 "InitializeConnectionSession"},
+    {0x00090040, nullptr,                 "BeginRequest"},
+    {0x000A0040, nullptr,                 "BeginRequestAsync"},
+    {0x000B0082, nullptr,                 "ReceiveData"},
+    {0x000C0102, nullptr,                 "ReceiveDataTimeout"},
+    {0x000D0146, nullptr,                 "SetProxy"},
+    {0x000E0040, nullptr,                 "SetProxyDefault"},
+    {0x000F00C4, nullptr,                 "SetBasicAuthorization"},
+    {0x00100080, nullptr,                 "SetSocketBufferSize"},
+    {0x001100C4, nullptr,                 "AddRequestHeader"},
+    {0x001200C4, nullptr,                 "AddPostDataAscii"},
+    {0x001300C4, nullptr,                 "AddPostDataBinary"},
+    {0x00140082, nullptr,                 "AddPostDataRaw"},
+    {0x00150080, nullptr,                 "SetPostDataType"},
+    {0x001600C4, nullptr,                 "SendPostDataAscii"},
+    {0x00170144, nullptr,                 "SendPostDataAsciiTimeout"},
+    {0x001800C4, nullptr,                 "SendPostDataBinary"},
+    {0x00190144, nullptr,                 "SendPostDataBinaryTimeout"},
+    {0x001A0082, nullptr,                 "SendPostDataRaw"},
+    {0x001B0102, nullptr,                 "SendPOSTDataRawTimeout"},
+    {0x001C0080, nullptr,                 "SetPostDataEncoding"},
+    {0x001D0040, nullptr,                 "NotifyFinishSendPostData"},
+    {0x001E00C4, nullptr,                 "GetResponseHeader"},
+    {0x001F0144, nullptr,                 "GetResponseHeaderTimeout"},
+    {0x00200082, nullptr,                 "GetResponseData"},
+    {0x00210102, nullptr,                 "GetResponseDataTimeout"},
+    {0x00220040, nullptr,                 "GetResponseStatusCode"},
+    {0x002300C0, nullptr,                 "GetResponseStatusCodeTimeout"},
+    {0x00240082, nullptr,                 "AddTrustedRootCA"},
+    {0x00350186, nullptr,                 "SetDefaultProxy"},
+    {0x00360000, nullptr,                 "ClearDNSCache"},
+    {0x00370080, nullptr,                 "SetKeepAlive"},
+    {0x003800C0, nullptr,                 "Finalize"},
+};
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// Interface class
+
+Interface::Interface() {
+    Register(FunctionTable, ARRAY_SIZE(FunctionTable));
+}
+
+} // namespace
diff --git a/src/core/hle/service/http_c.h b/src/core/hle/service/http_c.h
new file mode 100644
index 000000000..5ea3d1df3
--- /dev/null
+++ b/src/core/hle/service/http_c.h
@@ -0,0 +1,23 @@
+// Copyright 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#pragma once
+
+#include "core/hle/service/service.h"
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// Namespace HTTP_C
+
+namespace HTTP_C {
+
+class Interface : public Service::Interface {
+public:
+    Interface();
+
+    std::string GetPortName() const override {
+        return "http:C";
+    }
+};
+
+} // namespace
diff --git a/src/core/hle/service/ir_rst.cpp b/src/core/hle/service/ir_rst.cpp
index be15db231..b388afb15 100644
--- a/src/core/hle/service/ir_rst.cpp
+++ b/src/core/hle/service/ir_rst.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "common/log.h"
@@ -30,7 +30,4 @@ Interface::Interface() {
     Register(FunctionTable, ARRAY_SIZE(FunctionTable));
 }
 
-Interface::~Interface() {
-}
-
 } // namespace
diff --git a/src/core/hle/service/ir_rst.h b/src/core/hle/service/ir_rst.h
index 73effd7e3..deef701c5 100644
--- a/src/core/hle/service/ir_rst.h
+++ b/src/core/hle/service/ir_rst.h
@@ -1,6 +1,6 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
-// Refer to the license.txt file included.
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included..
 
 #pragma once
 
@@ -14,11 +14,7 @@ namespace IR_RST {
 class Interface : public Service::Interface {
 public:
     Interface();
-    ~Interface();
-    /**
-     * Gets the string port name used by CTROS for the service
-     * @return Port name of service
-     */
+
     std::string GetPortName() const override {
         return "ir:rst";
     }
diff --git a/src/core/hle/service/ir_u.cpp b/src/core/hle/service/ir_u.cpp
index aa9db6f6d..da6f38e41 100644
--- a/src/core/hle/service/ir_u.cpp
+++ b/src/core/hle/service/ir_u.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "common/log.h"
@@ -39,7 +39,4 @@ Interface::Interface() {
     Register(FunctionTable, ARRAY_SIZE(FunctionTable));
 }
 
-Interface::~Interface() {
-}
-
 } // namespace
diff --git a/src/core/hle/service/ir_u.h b/src/core/hle/service/ir_u.h
index 86d98d079..ec47a1524 100644
--- a/src/core/hle/service/ir_u.h
+++ b/src/core/hle/service/ir_u.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -14,11 +14,7 @@ namespace IR_U {
 class Interface : public Service::Interface {
 public:
     Interface();
-    ~Interface();
-    /**
-     * Gets the string port name used by CTROS for the service
-     * @return Port name of service
-     */
+
     std::string GetPortName() const override {
         return "ir:u";
     }
diff --git a/src/core/hle/service/ldr_ro.cpp b/src/core/hle/service/ldr_ro.cpp
new file mode 100644
index 000000000..9c9e90a40
--- /dev/null
+++ b/src/core/hle/service/ldr_ro.cpp
@@ -0,0 +1,29 @@
+// Copyright 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#include "common/log.h"
+#include "core/hle/hle.h"
+#include "core/hle/service/ldr_ro.h"
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// Namespace LDR_RO
+
+namespace LDR_RO {
+
+const Interface::FunctionInfo FunctionTable[] = {
+    {0x000100C2, nullptr,               "Initialize"},
+    {0x00020082, nullptr,               "CRR_Load"},
+    {0x00030042, nullptr,               "CRR_Unload"},
+    {0x000402C2, nullptr,               "CRO_LoadAndFix"},
+    {0x000500C2, nullptr,               "CRO_ApplyRelocationPatchesAndLink"}
+};
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// Interface class
+
+Interface::Interface() {
+    Register(FunctionTable, ARRAY_SIZE(FunctionTable));
+}
+
+} // namespace
diff --git a/src/core/hle/service/ldr_ro.h b/src/core/hle/service/ldr_ro.h
new file mode 100644
index 000000000..331637cde
--- /dev/null
+++ b/src/core/hle/service/ldr_ro.h
@@ -0,0 +1,23 @@
+// Copyright 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#pragma once
+
+#include "core/hle/service/service.h"
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// Namespace LDR_RO
+
+namespace LDR_RO {
+
+class Interface : public Service::Interface {
+public:
+    Interface();
+
+    std::string GetPortName() const override {
+        return "ldr:ro";
+    }
+};
+
+} // namespace
diff --git a/src/core/hle/service/mic_u.cpp b/src/core/hle/service/mic_u.cpp
index d6f30e9ae..82bce9180 100644
--- a/src/core/hle/service/mic_u.cpp
+++ b/src/core/hle/service/mic_u.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "common/log.h"
@@ -37,7 +37,4 @@ Interface::Interface() {
     Register(FunctionTable, ARRAY_SIZE(FunctionTable));
 }
 
-Interface::~Interface() {
-}
-
 } // namespace
diff --git a/src/core/hle/service/mic_u.h b/src/core/hle/service/mic_u.h
index 2a495f3a9..dc795d14c 100644
--- a/src/core/hle/service/mic_u.h
+++ b/src/core/hle/service/mic_u.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -16,11 +16,7 @@ namespace MIC_U {
 class Interface : public Service::Interface {
 public:
     Interface();
-    ~Interface();
-    /**
-     * Gets the string port name used by CTROS for the service
-     * @return Port name of service
-     */
+
     std::string GetPortName() const override {
         return "mic:u";
     }
diff --git a/src/core/hle/service/ndm_u.cpp b/src/core/hle/service/ndm_u.cpp
index 37c0661bf..233b14f6d 100644
--- a/src/core/hle/service/ndm_u.cpp
+++ b/src/core/hle/service/ndm_u.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "core/hle/hle.h"
@@ -24,7 +24,4 @@ Interface::Interface() {
     Register(FunctionTable, ARRAY_SIZE(FunctionTable));
 }
 
-Interface::~Interface() {
-}
-
 } // namespace
diff --git a/src/core/hle/service/ndm_u.h b/src/core/hle/service/ndm_u.h
index 2ca9fcf22..51c4b3902 100644
--- a/src/core/hle/service/ndm_u.h
+++ b/src/core/hle/service/ndm_u.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -15,19 +15,11 @@ namespace NDM_U {
 
 class Interface : public Service::Interface {
 public:
-
     Interface();
 
-    ~Interface();
-
-    /**
-     * Gets the string port name used by CTROS for the service
-     * @return Port name of service
-     */
     std::string GetPortName() const override {
         return "ndm:u";
     }
-
 };
 
 } // namespace
diff --git a/src/core/hle/service/news_u.cpp b/src/core/hle/service/news_u.cpp
new file mode 100644
index 000000000..b5adad4c6
--- /dev/null
+++ b/src/core/hle/service/news_u.cpp
@@ -0,0 +1,25 @@
+// Copyright 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#include "common/log.h"
+#include "core/hle/hle.h"
+#include "core/hle/service/news_u.h"
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// Namespace NEWS_U
+
+namespace NEWS_U {
+
+const Interface::FunctionInfo FunctionTable[] = {
+    {0x000100C8, nullptr,               "AddNotification"},
+};
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// Interface class
+
+Interface::Interface() {
+    Register(FunctionTable, ARRAY_SIZE(FunctionTable));
+}
+
+} // namespace
diff --git a/src/core/hle/service/news_u.h b/src/core/hle/service/news_u.h
new file mode 100644
index 000000000..0473cd19c
--- /dev/null
+++ b/src/core/hle/service/news_u.h
@@ -0,0 +1,23 @@
+// Copyright 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#pragma once
+
+#include "core/hle/service/service.h"
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// Namespace NEWS_U
+
+namespace NEWS_U {
+
+class Interface : public Service::Interface {
+public:
+    Interface();
+
+    std::string GetPortName() const override {
+        return "news:u";
+    }
+};
+
+} // namespace
diff --git a/src/core/hle/service/nim_aoc.cpp b/src/core/hle/service/nim_aoc.cpp
new file mode 100644
index 000000000..17d1c4ff5
--- /dev/null
+++ b/src/core/hle/service/nim_aoc.cpp
@@ -0,0 +1,31 @@
+// Copyright 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#include "common/log.h"
+#include "core/hle/hle.h"
+#include "core/hle/service/nim_aoc.h"
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// Namespace NIM_AOC
+
+namespace NIM_AOC {
+
+const Interface::FunctionInfo FunctionTable[] = {
+    {0x00030042, nullptr,               "SetApplicationId"},
+    {0x00040042, nullptr,               "SetTin"},
+    {0x000902D0, nullptr,               "ListContentSetsEx"},
+    {0x00180000, nullptr,               "GetBalance"},
+    {0x001D0000, nullptr,               "GetCustomerSupportCode"},
+    {0x00210000, nullptr,               "Initialize"},
+    {0x00240282, nullptr,               "CalculateContentsRequiredSize"},
+    {0x00250000, nullptr,               "RefreshServerTime"},
+};
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// Interface class
+
+Interface::Interface() {
+    Register(FunctionTable, ARRAY_SIZE(FunctionTable));
+}
+
+} // namespace
diff --git a/src/core/hle/service/nim_aoc.h b/src/core/hle/service/nim_aoc.h
new file mode 100644
index 000000000..aeb71eed2
--- /dev/null
+++ b/src/core/hle/service/nim_aoc.h
@@ -0,0 +1,23 @@
+// Copyright 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#pragma once
+
+#include "core/hle/service/service.h"
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+// Namespace NIM_AOC
+
+namespace NIM_AOC {
+
+class Interface : public Service::Interface {
+public:
+    Interface();
+
+    std::string GetPortName() const override {
+        return "nim:aoc";
+    }
+};
+
+} // namespace
diff --git a/src/core/hle/service/nwm_uds.cpp b/src/core/hle/service/nwm_uds.cpp
index 14df86d85..ce456a966 100644
--- a/src/core/hle/service/nwm_uds.cpp
+++ b/src/core/hle/service/nwm_uds.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "common/log.h"
@@ -29,7 +29,4 @@ Interface::Interface() {
     Register(FunctionTable, ARRAY_SIZE(FunctionTable));
 }
 
-Interface::~Interface() {
-}
-
 } // namespace
diff --git a/src/core/hle/service/nwm_uds.h b/src/core/hle/service/nwm_uds.h
index 69d2c2002..9043f5aa7 100644
--- a/src/core/hle/service/nwm_uds.h
+++ b/src/core/hle/service/nwm_uds.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -16,11 +16,7 @@ namespace NWM_UDS {
 class Interface : public Service::Interface {
 public:
     Interface();
-    ~Interface();
-    /**
-     * Gets the string port name used by CTROS for the service
-     * @return Port name of service
-     */
+
     std::string GetPortName() const override {
         return "nwm:UDS";
     }
diff --git a/src/core/hle/service/pm_app.cpp b/src/core/hle/service/pm_app.cpp
index 90e9b1bfa..529dccafb 100644
--- a/src/core/hle/service/pm_app.cpp
+++ b/src/core/hle/service/pm_app.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "common/log.h"
@@ -29,7 +29,4 @@ Interface::Interface() {
     Register(FunctionTable, ARRAY_SIZE(FunctionTable));
 }
 
-Interface::~Interface() {
-}
-
 } // namespace
diff --git a/src/core/hle/service/pm_app.h b/src/core/hle/service/pm_app.h
index 28c38f582..c1fb1f9da 100644
--- a/src/core/hle/service/pm_app.h
+++ b/src/core/hle/service/pm_app.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -14,11 +14,7 @@ namespace PM_APP {
 class Interface : public Service::Interface {
 public:
     Interface();
-    ~Interface();
-    /**
-     * Gets the string port name used by CTROS for the service
-     * @return Port name of service
-     */
+
     std::string GetPortName() const override {
         return "pm:app";
     }
diff --git a/src/core/hle/service/ptm_u.cpp b/src/core/hle/service/ptm_u.cpp
index 559f148dd..d1498f05c 100644
--- a/src/core/hle/service/ptm_u.cpp
+++ b/src/core/hle/service/ptm_u.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "common/log.h"
@@ -34,7 +34,7 @@ static bool battery_is_charging = true;
  *      2 : Output of function, 0 = not charging, 1 = charging.
  */
 static void GetAdapterState(Service::Interface* self) {
-    u32* cmd_buff = Service::GetCommandBuffer();
+    u32* cmd_buff = Kernel::GetCommandBuffer();
 
     // TODO(purpasmart96): This function is only a stub,
     // it returns a valid result without implementing full functionality.
@@ -52,7 +52,7 @@ static void GetAdapterState(Service::Interface* self) {
  *      2 : Whether the 3DS's physical shell casing is open (1) or closed (0)
  */
 static void GetShellState(Service::Interface* self) {
-    u32* cmd_buff = Service::GetCommandBuffer();
+    u32* cmd_buff = Kernel::GetCommandBuffer();
 
     cmd_buff[1] = 0;
     cmd_buff[2] = shell_open ? 1 : 0;
@@ -68,7 +68,7 @@ static void GetShellState(Service::Interface* self) {
  *          3 = half full battery, 2 =  low battery, 1 = critical battery.
  */
 static void GetBatteryLevel(Service::Interface* self) {
-    u32* cmd_buff = Service::GetCommandBuffer();
+    u32* cmd_buff = Kernel::GetCommandBuffer();
 
     // TODO(purpasmart96): This function is only a stub,
     // it returns a valid result without implementing full functionality.
@@ -86,7 +86,7 @@ static void GetBatteryLevel(Service::Interface* self) {
  *      2 : Output of function, 0 = not charging, 1 = charging.
  */
 static void GetBatteryChargeState(Service::Interface* self) {
-    u32* cmd_buff = Service::GetCommandBuffer();
+    u32* cmd_buff = Kernel::GetCommandBuffer();
 
     // TODO(purpasmart96): This function is only a stub,
     // it returns a valid result without implementing full functionality.
@@ -122,7 +122,4 @@ Interface::Interface() {
     Register(FunctionTable, ARRAY_SIZE(FunctionTable));
 }
 
-Interface::~Interface() {
-}
-
 } // namespace
diff --git a/src/core/hle/service/ptm_u.h b/src/core/hle/service/ptm_u.h
index f8d9f57be..a44624fd5 100644
--- a/src/core/hle/service/ptm_u.h
+++ b/src/core/hle/service/ptm_u.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -16,11 +16,7 @@ namespace PTM_U {
 class Interface : public Service::Interface {
 public:
     Interface();
-    ~Interface();
-    /**
-     * Gets the string port name used by CTROS for the service
-     * @return Port name of service
-     */
+
     std::string GetPortName() const override {
         return "ptm:u";
     }
diff --git a/src/core/hle/service/service.cpp b/src/core/hle/service/service.cpp
index e6973572b..0f3cc2aa8 100644
--- a/src/core/hle/service/service.cpp
+++ b/src/core/hle/service/service.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "common/common.h"
@@ -7,22 +7,30 @@
 
 #include "core/hle/service/service.h"
 #include "core/hle/service/ac_u.h"
+#include "core/hle/service/act_u.h"
+#include "core/hle/service/am_app.h"
 #include "core/hle/service/am_net.h"
+#include "core/hle/service/apt_a.h"
 #include "core/hle/service/apt_u.h"
 #include "core/hle/service/boss_u.h"
-#include "core/hle/service/cfg_i.h"
-#include "core/hle/service/cfg_u.h"
+#include "core/hle/service/cecd_u.h"
+#include "core/hle/service/cfg/cfg_i.h"
+#include "core/hle/service/cfg/cfg_u.h"
 #include "core/hle/service/csnd_snd.h"
 #include "core/hle/service/dsp_dsp.h"
 #include "core/hle/service/err_f.h"
-#include "core/hle/service/fs_user.h"
+#include "core/hle/service/fs/fs_user.h"
 #include "core/hle/service/frd_u.h"
 #include "core/hle/service/gsp_gpu.h"
 #include "core/hle/service/hid_user.h"
+#include "core/hle/service/http_c.h"
 #include "core/hle/service/ir_rst.h"
 #include "core/hle/service/ir_u.h"
+#include "core/hle/service/ldr_ro.h"
 #include "core/hle/service/mic_u.h"
 #include "core/hle/service/ndm_u.h"
+#include "core/hle/service/news_u.h"
+#include "core/hle/service/nim_aoc.h"
 #include "core/hle/service/nwm_uds.h"
 #include "core/hle/service/pm_app.h"
 #include "core/hle/service/ptm_u.h"
@@ -48,7 +56,8 @@ Manager::~Manager() {
 
 /// Add a service to the manager (does not create it though)
 void Manager::AddService(Interface* service) {
-    m_port_map[service->GetPortName()] = Kernel::g_object_pool.Create(service);
+    // TOOD(yuriks): Fix error reporting
+    m_port_map[service->GetPortName()] = Kernel::g_handle_table.Create(service).ValueOr(INVALID_HANDLE);
     m_services.push_back(service);
 }
 
@@ -62,7 +71,7 @@ void Manager::DeleteService(const std::string& port_name) {
 
 /// Get a Service Interface from its Handle
 Interface* Manager::FetchFromHandle(Handle handle) {
-    return Kernel::g_object_pool.Get<Interface>(handle);
+    return Kernel::g_handle_table.Get<Interface>(handle);
 }
 
 /// Get a Service Interface from its port
@@ -84,22 +93,30 @@ void Init() {
 
     g_manager->AddService(new SRV::Interface);
     g_manager->AddService(new AC_U::Interface);
+    g_manager->AddService(new ACT_U::Interface);
+    g_manager->AddService(new AM_APP::Interface);
     g_manager->AddService(new AM_NET::Interface);
+    g_manager->AddService(new APT_A::Interface);
     g_manager->AddService(new APT_U::Interface);
     g_manager->AddService(new BOSS_U::Interface);
+    g_manager->AddService(new CECD_U::Interface);
     g_manager->AddService(new CFG_I::Interface);
     g_manager->AddService(new CFG_U::Interface);
     g_manager->AddService(new CSND_SND::Interface);
     g_manager->AddService(new DSP_DSP::Interface);
     g_manager->AddService(new ERR_F::Interface);
     g_manager->AddService(new FRD_U::Interface);
-    g_manager->AddService(new FS_User::Interface);
+    g_manager->AddService(new FS::FSUserInterface);
     g_manager->AddService(new GSP_GPU::Interface);
     g_manager->AddService(new HID_User::Interface);
+    g_manager->AddService(new HTTP_C::Interface);
     g_manager->AddService(new IR_RST::Interface);
     g_manager->AddService(new IR_U::Interface);
+    g_manager->AddService(new LDR_RO::Interface);
     g_manager->AddService(new MIC_U::Interface);
     g_manager->AddService(new NDM_U::Interface);
+    g_manager->AddService(new NEWS_U::Interface);
+    g_manager->AddService(new NIM_AOC::Interface);
     g_manager->AddService(new NWM_UDS::Interface);
     g_manager->AddService(new PM_APP::Interface);
     g_manager->AddService(new PTM_U::Interface);
diff --git a/src/core/hle/service/service.h b/src/core/hle/service/service.h
index baae910a1..28b4ccd17 100644
--- a/src/core/hle/service/service.h
+++ b/src/core/hle/service/service.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -14,6 +14,7 @@
 #include "core/mem_map.h"
 
 #include "core/hle/kernel/kernel.h"
+#include "core/hle/kernel/session.h"
 #include "core/hle/svc.h"
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -21,30 +22,19 @@
 
 namespace Service {
 
-static const int kMaxPortSize           = 0x08; ///< Maximum size of a port name (8 characters)
-static const int kCommandHeaderOffset   = 0x80; ///< Offset into command buffer of header
-
-/**
- * Returns a pointer to the command buffer in kernel memory
- * @param offset Optional offset into command buffer
- * @return Pointer to command buffer
- */
-inline static u32* GetCommandBuffer(const int offset=0) {
-    return (u32*)Memory::GetPointer(Memory::KERNEL_MEMORY_VADDR + kCommandHeaderOffset + offset);
-}
+static const int kMaxPortSize = 8; ///< Maximum size of a port name (8 characters)
 
 class Manager;
 
 /// Interface to a CTROS service
-class Interface  : public Kernel::Object {
+class Interface  : public Kernel::Session {
+    // TODO(yuriks): An "Interface" being a Kernel::Object is mostly non-sense. Interface should be
+    // just something that encapsulates a session and acts as a helper to implement service
+    // processes.
+
     friend class Manager;
 public:
-
     std::string GetName() const override { return GetPortName(); }
-    std::string GetTypeName() const override { return GetPortName(); }
-
-    static Kernel::HandleType GetStaticHandleType() { return Kernel::HandleType::Service; }
-    Kernel::HandleType GetHandleType() const override { return Kernel::HandleType::Service; }
 
     typedef void (*Function)(Interface*);
 
@@ -64,7 +54,8 @@ public:
 
     /// Allocates a new handle for the service
     Handle CreateHandle(Kernel::Object *obj) {
-        Handle handle = Kernel::g_object_pool.Create(obj);
+        // TODO(yuriks): Fix error reporting
+        Handle handle = Kernel::g_handle_table.Create(obj).ValueOr(INVALID_HANDLE);
         m_handles.push_back(handle);
         return handle;
     }
@@ -72,12 +63,12 @@ public:
     /// Frees a handle from the service
     template <class T>
     void DeleteHandle(const Handle handle) {
-        Kernel::g_object_pool.Destroy<T>(handle);
+        Kernel::g_handle_table.Close(handle);
         m_handles.erase(std::remove(m_handles.begin(), m_handles.end(), handle), m_handles.end());
     }
 
     ResultVal<bool> SyncRequest() override {
-        u32* cmd_buff = GetCommandBuffer();
+        u32* cmd_buff = Kernel::GetCommandBuffer();
         auto itr = m_functions.find(cmd_buff[0]);
 
         if (itr == m_functions.end() || itr->second.func == nullptr) {
diff --git a/src/core/hle/service/soc_u.cpp b/src/core/hle/service/soc_u.cpp
index 2f8910468..ef4f9829d 100644
--- a/src/core/hle/service/soc_u.cpp
+++ b/src/core/hle/service/soc_u.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "common/log.h"
@@ -52,7 +52,4 @@ Interface::Interface() {
     Register(FunctionTable, ARRAY_SIZE(FunctionTable));
 }
 
-Interface::~Interface() {
-}
-
 } // namespace
diff --git a/src/core/hle/service/soc_u.h b/src/core/hle/service/soc_u.h
index d5590a683..2edf3b482 100644
--- a/src/core/hle/service/soc_u.h
+++ b/src/core/hle/service/soc_u.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -14,11 +14,7 @@ namespace SOC_U {
 class Interface : public Service::Interface {
 public:
     Interface();
-    ~Interface();
-    /**
-     * Gets the string port name used by CTROS for the service
-     * @return Port name of service
-     */
+
     std::string GetPortName() const override {
         return "soc:U";
     }
diff --git a/src/core/hle/service/srv.cpp b/src/core/hle/service/srv.cpp
index 24a846533..25fab1a4f 100644
--- a/src/core/hle/service/srv.cpp
+++ b/src/core/hle/service/srv.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "core/hle/hle.h"
@@ -16,7 +16,7 @@ static Handle g_event_handle = 0;
 static void Initialize(Service::Interface* self) {
     LOG_DEBUG(Service_SRV, "called");
 
-    u32* cmd_buff = Service::GetCommandBuffer();
+    u32* cmd_buff = Kernel::GetCommandBuffer();
 
     cmd_buff[1] = 0; // No error
 }
@@ -24,7 +24,7 @@ static void Initialize(Service::Interface* self) {
 static void GetProcSemaphore(Service::Interface* self) {
     LOG_TRACE(Service_SRV, "called");
 
-    u32* cmd_buff = Service::GetCommandBuffer();
+    u32* cmd_buff = Kernel::GetCommandBuffer();
 
     // TODO(bunnei): Change to a semaphore once these have been implemented
     g_event_handle = Kernel::CreateEvent(RESETTYPE_ONESHOT, "SRV:Event");
@@ -36,7 +36,7 @@ static void GetProcSemaphore(Service::Interface* self) {
 
 static void GetServiceHandle(Service::Interface* self) {
     ResultCode res = RESULT_SUCCESS;
-    u32* cmd_buff = Service::GetCommandBuffer();
+    u32* cmd_buff = Kernel::GetCommandBuffer();
 
     std::string port_name = std::string((const char*)&cmd_buff[1], 0, Service::kMaxPortSize);
     Service::Interface* service = Service::g_manager->FetchFromPortName(port_name);
@@ -68,7 +68,4 @@ Interface::Interface() {
     Register(FunctionTable, ARRAY_SIZE(FunctionTable));
 }
 
-Interface::~Interface() {
-}
-
 } // namespace
diff --git a/src/core/hle/service/srv.h b/src/core/hle/service/srv.h
index 6d5fe5048..653aba5cb 100644
--- a/src/core/hle/service/srv.h
+++ b/src/core/hle/service/srv.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "core/hle/service/service.h"
@@ -11,21 +11,12 @@ namespace SRV {
 
 /// Interface to "srv:" service
 class Interface : public Service::Interface {
-
 public:
-
     Interface();
 
-    ~Interface();
-
-    /**
-     * Gets the string name used by CTROS for the service
-     * @return Port name of service
-     */
     std::string GetPortName() const override {
         return "srv:";
     }
-
 };
 
 } // namespace
diff --git a/src/core/hle/service/ssl_c.cpp b/src/core/hle/service/ssl_c.cpp
index 4aa660ecc..360516cdf 100644
--- a/src/core/hle/service/ssl_c.cpp
+++ b/src/core/hle/service/ssl_c.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "common/log.h"
@@ -25,7 +25,4 @@ Interface::Interface() {
     Register(FunctionTable, ARRAY_SIZE(FunctionTable));
 }
 
-Interface::~Interface() {
-}
-
 } // namespace
diff --git a/src/core/hle/service/ssl_c.h b/src/core/hle/service/ssl_c.h
index 7b4e7fd8a..58e87c1cb 100644
--- a/src/core/hle/service/ssl_c.h
+++ b/src/core/hle/service/ssl_c.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -14,12 +14,8 @@ namespace SSL_C {
 class Interface : public Service::Interface {
 public:
     Interface();
-    ~Interface();
-    /**
-     * Gets the string port name used by CTROS for the service
-     * @return Port name of service
-     */
-    std::string GetPortName() const {
+
+    std::string GetPortName() const override {
         return "ssl:C";
     }
 };
diff --git a/src/core/hle/svc.cpp b/src/core/hle/svc.cpp
index db0c42e74..25944fc68 100644
--- a/src/core/hle/svc.cpp
+++ b/src/core/hle/svc.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include <map>
@@ -12,6 +12,7 @@
 #include "core/hle/kernel/address_arbiter.h"
 #include "core/hle/kernel/event.h"
 #include "core/hle/kernel/mutex.h"
+#include "core/hle/kernel/semaphore.h"
 #include "core/hle/kernel/shared_memory.h"
 #include "core/hle/kernel/thread.h"
 
@@ -63,6 +64,10 @@ static Result MapMemoryBlock(Handle handle, u32 addr, u32 permissions, u32 other
     case Kernel::MemoryPermission::Read:
     case Kernel::MemoryPermission::Write:
     case Kernel::MemoryPermission::ReadWrite:
+    case Kernel::MemoryPermission::Execute:
+    case Kernel::MemoryPermission::ReadExecute:
+    case Kernel::MemoryPermission::WriteExecute:
+    case Kernel::MemoryPermission::ReadWriteExecute:
     case Kernel::MemoryPermission::DontCare:
         Kernel::MapSharedMemory(handle, addr, permissions_type,
             static_cast<Kernel::MemoryPermission>(other_permissions));
@@ -87,17 +92,14 @@ static Result ConnectToPort(Handle* out, const char* port_name) {
 
 /// Synchronize to an OS service
 static Result SendSyncRequest(Handle handle) {
-    // TODO(yuriks): ObjectPool::Get tries to check the Object type, which fails since this is a generic base Object,
-    // so we are forced to use GetFast and manually verify the handle.
-    if (!Kernel::g_object_pool.IsValid(handle)) {
+    Kernel::Session* session = Kernel::g_handle_table.Get<Kernel::Session>(handle);
+    if (session == nullptr) {
         return InvalidHandle(ErrorModule::Kernel).raw;
     }
-    Kernel::Object* object = Kernel::g_object_pool.GetFast<Kernel::Object>(handle);
 
-    _assert_msg_(KERNEL, (object != nullptr), "called, but kernel object is nullptr!");
-    LOG_TRACE(Kernel_SVC, "called handle=0x%08X(%s)", handle, object->GetTypeName().c_str());
+    LOG_TRACE(Kernel_SVC, "called handle=0x%08X(%s)", handle, session->GetName().c_str());
 
-    ResultVal<bool> wait = object->SyncRequest();
+    ResultVal<bool> wait = session->SyncRequest();
     if (wait.Succeeded() && *wait) {
         Kernel::WaitCurrentThread(WAITTYPE_SYNCH); // TODO(bunnei): Is this correct?
     }
@@ -117,11 +119,9 @@ static Result WaitSynchronization1(Handle handle, s64 nano_seconds) {
     // TODO(bunnei): Do something with nano_seconds, currently ignoring this
     bool wait_infinite = (nano_seconds == -1); // Used to wait until a thread has terminated
 
-    if (!Kernel::g_object_pool.IsValid(handle)) {
+    Kernel::Object* object = Kernel::g_handle_table.GetGeneric(handle);
+    if (object == nullptr)
         return InvalidHandle(ErrorModule::Kernel).raw;
-    }
-    Kernel::Object* object = Kernel::g_object_pool.GetFast<Kernel::Object>(handle);
-    _dbg_assert_(Kernel, object != nullptr);
 
     LOG_TRACE(Kernel_SVC, "called handle=0x%08X(%s:%s), nanoseconds=%lld", handle, object->GetTypeName().c_str(),
             object->GetName().c_str(), nano_seconds);
@@ -148,10 +148,9 @@ static Result WaitSynchronizationN(s32* out, Handle* handles, s32 handle_count,
 
     // Iterate through each handle, synchronize kernel object
     for (s32 i = 0; i < handle_count; i++) {
-        if (!Kernel::g_object_pool.IsValid(handles[i])) {
+        Kernel::Object* object = Kernel::g_handle_table.GetGeneric(handles[i]);
+        if (object == nullptr)
             return InvalidHandle(ErrorModule::Kernel).raw;
-        }
-        Kernel::Object* object = Kernel::g_object_pool.GetFast<Kernel::Object>(handles[i]);
 
         LOG_TRACE(Kernel_SVC, "\thandle[%d] = 0x%08X(%s:%s)", i, handles[i], object->GetTypeName().c_str(),
             object->GetName().c_str());
@@ -288,6 +287,21 @@ static Result GetThreadId(u32* thread_id, Handle handle) {
     return result.raw;
 }
 
+/// Creates a semaphore
+static Result CreateSemaphore(Handle* semaphore, s32 initial_count, s32 max_count) {
+    ResultCode res = Kernel::CreateSemaphore(semaphore, initial_count, max_count);
+    LOG_TRACE(Kernel_SVC, "called initial_count=%d, max_count=%d, created handle=0x%08X",
+        initial_count, max_count, *semaphore);
+    return res.raw;
+}
+
+/// Releases a certain number of slots in a semaphore
+static Result ReleaseSemaphore(s32* count, Handle semaphore, s32 release_count) {
+    LOG_TRACE(Kernel_SVC, "called release_count=%d, handle=0x%08X", release_count, semaphore);
+    ResultCode res = Kernel::ReleaseSemaphore(count, semaphore, release_count);
+    return res.raw;
+}
+
 /// Query memory
 static Result QueryMemory(void* info, void* out, u32 addr) {
     LOG_ERROR(Kernel_SVC, "(UNIMPLEMENTED) called addr=0x%08X", addr);
@@ -304,19 +318,12 @@ static Result CreateEvent(Handle* evt, u32 reset_type) {
 
 /// Duplicates a kernel handle
 static Result DuplicateHandle(Handle* out, Handle handle) {
-    LOG_WARNING(Kernel_SVC, "(STUBBED) called handle=0x%08X", handle);
-
-    // Translate kernel handles -> real handles
-    if (handle == Kernel::CurrentThread) {
-        handle = Kernel::GetCurrentThreadHandle();
+    ResultVal<Handle> out_h = Kernel::g_handle_table.Duplicate(handle);
+    if (out_h.Succeeded()) {
+        *out = *out_h;
+        LOG_TRACE(Kernel_SVC, "duplicated 0x%08X to 0x%08X", handle, *out);
     }
-    _assert_msg_(KERNEL, (handle != Kernel::CurrentProcess),
-        "(UNIMPLEMENTED) process handle duplication!");
-
-    // TODO(bunnei): FixMe - This is a hack to return the handle that we were asked to duplicate.
-    *out = handle;
-
-    return 0;
+    return out_h.Code().raw;
 }
 
 /// Signals an event
@@ -335,7 +342,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__);
 }
 
@@ -366,8 +374,8 @@ const HLE::FunctionDef SVC_Table[] = {
     {0x12, nullptr,                         "Run"},
     {0x13, HLE::Wrap<CreateMutex>,          "CreateMutex"},
     {0x14, HLE::Wrap<ReleaseMutex>,         "ReleaseMutex"},
-    {0x15, nullptr,                         "CreateSemaphore"},
-    {0x16, nullptr,                         "ReleaseSemaphore"},
+    {0x15, HLE::Wrap<CreateSemaphore>,      "CreateSemaphore"},
+    {0x16, HLE::Wrap<ReleaseSemaphore>,     "ReleaseSemaphore"},
     {0x17, HLE::Wrap<CreateEvent>,          "CreateEvent"},
     {0x18, HLE::Wrap<SignalEvent>,          "SignalEvent"},
     {0x19, HLE::Wrap<ClearEvent>,           "ClearEvent"},
diff --git a/src/core/hle/svc.h b/src/core/hle/svc.h
index 6be393d0b..ad780818e 100644
--- a/src/core/hle/svc.h
+++ b/src/core/hle/svc.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/core/hw/gpu.cpp b/src/core/hw/gpu.cpp
index da78b85e5..dd619cb16 100644
--- a/src/core/hw/gpu.cpp
+++ b/src/core/hw/gpu.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "common/common_types.h"
@@ -21,12 +21,14 @@ namespace GPU {
 
 Regs g_regs;
 
-u32 g_cur_line = 0;         ///< Current vertical screen line
-u64 g_last_line_ticks = 0;  ///< CPU tick count from last vertical screen line
-u64 g_last_frame_ticks = 0; ///< CPU tick count from last frame
+bool g_skip_frame = false;              ///< True if the current frame was skipped
 
-static u32 kFrameCycles = 0; ///< 268MHz / 60 frames per second
-static u32 kFrameTicks  = 0; ///< Approximate number of instructions/frame
+static u64 frame_ticks      = 0;        ///< 268MHz / gpu_refresh_rate frames per second
+static u64 line_ticks       = 0;        ///< Number of ticks for a screen line
+static u32 cur_line         = 0;        ///< Current screen line
+static u64 last_update_tick = 0;        ///< CPU ticl count from last GPU update
+static u64 frame_count      = 0;        ///< Number of frames drawn
+static bool last_skip_frame = false;    ///< True if the last frame was skipped
 
 template <typename T>
 inline void Read(T &var, const u32 raw_addr) {
@@ -34,7 +36,7 @@ inline void Read(T &var, const u32 raw_addr) {
     u32 index = addr / 4;
 
     // Reads other than u32 are untested, so I'd rather have them abort than silently fail
-    if (index >= Regs::NumIds() || !std::is_same<T,u32>::value) {
+    if (index >= Regs::NumIds() || !std::is_same<T, u32>::value) {
         LOG_ERROR(HW_GPU, "unknown Read%lu @ 0x%08X", sizeof(var) * 8, addr);
         return;
     }
@@ -48,7 +50,7 @@ inline void Write(u32 addr, const T data) {
     u32 index = addr / 4;
 
     // Writes other than u32 are untested, so I'd rather have them abort than silently fail
-    if (index >= Regs::NumIds() || !std::is_same<T,u32>::value) {
+    if (index >= Regs::NumIds() || !std::is_same<T, u32>::value) {
         LOG_ERROR(HW_GPU, "unknown Write%lu 0x%08X @ 0x%08X", sizeof(data) * 8, (u32)data, addr);
         return;
     }
@@ -179,37 +181,41 @@ template void Write<u8>(u32 addr, const u8 data);
 /// Update hardware
 void Update() {
     auto& framebuffer_top = g_regs.framebuffer_config[0];
-    u64 current_ticks = Core::g_app_core->GetTicks();
-
-    // Update the frame after a certain number of CPU ticks have elapsed. This assumes that the
-    // active frame in memory is always complete to render. There also may be issues with this
-    // becoming out-of-synch with GSP synchrinization code (as follows). At this time, this seems to
-    // be the most effective solution for both homebrew and retail applications. With retail, this
-    // could be moved below (and probably would guarantee more accurate synchronization). However,
-    // primitive homebrew relies on a vertical blank interrupt to happen inevitably (regardless of a
-    // threading reschedule).
-
-    if ((current_ticks - g_last_frame_ticks) > GPU::kFrameTicks) {
-        VideoCore::g_renderer->SwapBuffers();
-        g_last_frame_ticks = current_ticks;
-    }
 
     // Synchronize GPU on a thread reschedule: Because we cannot accurately predict a vertical
     // blank, we need to simulate it. Based on testing, it seems that retail applications work more
     // accurately when this is signalled between thread switches.
 
     if (HLE::g_reschedule) {
+        u64 current_ticks = Core::g_app_core->GetTicks();
+        u32 num_lines = static_cast<u32>((current_ticks - last_update_tick) / line_ticks);
 
         // Synchronize line...
-        if ((current_ticks - g_last_line_ticks) >= GPU::kFrameTicks / framebuffer_top.height) {
+        if (num_lines > 0) {
             GSP_GPU::SignalInterrupt(GSP_GPU::InterruptId::PDC0);
-            g_cur_line++;
-            g_last_line_ticks = current_ticks;
+            cur_line += num_lines;
+            last_update_tick += (num_lines * line_ticks);
         }
 
         // Synchronize frame...
-        if (g_cur_line >= framebuffer_top.height) {
-            g_cur_line = 0;
+        if (cur_line >= framebuffer_top.height) {
+            cur_line = 0;
+            frame_count++;
+            last_skip_frame = g_skip_frame;
+            g_skip_frame = (frame_count & Settings::values.frame_skip) != 0;
+
+            // Swap buffers based on the frameskip mode, which is a little bit tricky. When
+            // a frame is being skipped, nothing is being rendered to the internal framebuffer(s).
+            // So, we should only swap frames if the last frame was rendered. The rules are:
+            //  - If frameskip == 0 (disabled), always swap buffers
+            //  - If frameskip == 1, swap buffers every other frame (starting from the first frame)
+            //  - If frameskip > 1, swap buffers every frameskip^n frames (starting from the second frame)
+
+            if ((((Settings::values.frame_skip != 1) ^ last_skip_frame) && last_skip_frame != g_skip_frame) || 
+                   Settings::values.frame_skip == 0) {
+                VideoCore::g_renderer->SwapBuffers();
+            }
+
             GSP_GPU::SignalInterrupt(GSP_GPU::InterruptId::PDC1);
         }
     }
@@ -217,12 +223,6 @@ void Update() {
 
 /// Initialize hardware
 void Init() {
-    kFrameCycles = 268123480 / Settings::values.gpu_refresh_rate;
-    kFrameTicks  = kFrameCycles / 3;
-
-    g_cur_line = 0;
-    g_last_frame_ticks = g_last_line_ticks = Core::g_app_core->GetTicks();
-
     auto& framebuffer_top = g_regs.framebuffer_config[0];
     auto& framebuffer_sub = g_regs.framebuffer_config[1];
 
@@ -251,6 +251,13 @@ void Init() {
     framebuffer_sub.color_format = Regs::PixelFormat::RGB8;
     framebuffer_sub.active_fb = 0;
 
+    frame_ticks = 268123480 / Settings::values.gpu_refresh_rate;
+    line_ticks = (GPU::frame_ticks / framebuffer_top.height);
+    cur_line = 0;
+    last_update_tick = Core::g_app_core->GetTicks();
+    last_skip_frame = false;
+    g_skip_frame = false;
+
     LOG_DEBUG(HW_GPU, "initialized OK");
 }
 
diff --git a/src/core/hw/gpu.h b/src/core/hw/gpu.h
index 86cd5e680..292f496c1 100644
--- a/src/core/hw/gpu.h
+++ b/src/core/hw/gpu.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -241,6 +241,7 @@ ASSERT_REG_POSITION(command_processor_config, 0x00638);
 static_assert(sizeof(Regs) == 0x1000 * sizeof(u32), "Invalid total size of register set");
 
 extern Regs g_regs;
+extern bool g_skip_frame;
 
 template <typename T>
 void Read(T &var, const u32 addr);
diff --git a/src/core/hw/hw.cpp b/src/core/hw/hw.cpp
index af42b41fb..848ab5348 100644
--- a/src/core/hw/hw.cpp
+++ b/src/core/hw/hw.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "common/common_types.h"
diff --git a/src/core/hw/hw.h b/src/core/hw/hw.h
index 1055ed94f..991c0a07d 100644
--- a/src/core/hw/hw.h
+++ b/src/core/hw/hw.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/core/loader/3dsx.cpp b/src/core/loader/3dsx.cpp
index f48d13530..4d072871a 100644
--- a/src/core/loader/3dsx.cpp
+++ b/src/core/loader/3dsx.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2+
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include <algorithm>
@@ -8,7 +8,7 @@
 #include "core/file_sys/archive_romfs.h"
 #include "core/loader/elf.h"
 #include "core/loader/ncch.h"
-#include "core/hle/kernel/archive.h"
+#include "core/hle/service/fs/archive.h"
 #include "core/mem_map.h"
 
 #include "3dsx.h"
@@ -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;
diff --git a/src/core/loader/3dsx.h b/src/core/loader/3dsx.h
index 848d3ef8a..da8836662 100644
--- a/src/core/loader/3dsx.h
+++ b/src/core/loader/3dsx.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Dolphin Emulator Project / Citra Emulator Project
-// Licensed under GPLv2+
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/core/loader/elf.cpp b/src/core/loader/elf.cpp
index c95882f4a..354335014 100644
--- a/src/core/loader/elf.cpp
+++ b/src/core/loader/elf.cpp
@@ -1,5 +1,5 @@
-// Copyright 2013 Dolphin Emulator Project
-// Licensed under GPLv2
+// Copyright 2013 Dolphin Emulator Project / 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include <string>
diff --git a/src/core/loader/elf.h b/src/core/loader/elf.h
index 5ae88439a..c221cce6d 100644
--- a/src/core/loader/elf.h
+++ b/src/core/loader/elf.h
@@ -1,5 +1,5 @@
-// Copyright 2013 Dolphin Emulator Project / Citra Emulator Project
-// Licensed under GPLv2
+// Copyright 2013 Dolphin Emulator Project / 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/core/loader/loader.cpp b/src/core/loader/loader.cpp
index 3883e1307..87580cb2a 100644
--- a/src/core/loader/loader.cpp
+++ b/src/core/loader/loader.cpp
@@ -1,14 +1,16 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
-#include <memory>
+#include <string>
+
+#include "common/make_unique.h"
 
 #include "core/file_sys/archive_romfs.h"
 #include "core/loader/3dsx.h"
 #include "core/loader/elf.h"
 #include "core/loader/ncch.h"
-#include "core/hle/kernel/archive.h"
+#include "core/hle/service/fs/archive.h"
 #include "core/mem_map.h"
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -74,7 +76,8 @@ ResultStatus LoadFile(const std::string& filename) {
 
         // Load application and RomFS
         if (ResultStatus::Success == app_loader.Load()) {
-            Kernel::CreateArchive(new FileSys::Archive_RomFS(app_loader), "RomFS");
+            Kernel::g_program_id = app_loader.GetProgramId();
+            Service::FS::CreateArchive(Common::make_unique<FileSys::Archive_RomFS>(app_loader), Service::FS::ArchiveIdCode::RomFS);
             return ResultStatus::Success;
         }
         break;
diff --git a/src/core/loader/loader.h b/src/core/loader/loader.h
index 0f836d285..ec5534d41 100644
--- a/src/core/loader/loader.h
+++ b/src/core/loader/loader.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/core/loader/ncch.cpp b/src/core/loader/ncch.cpp
index ba9ba00c0..0dc21699e 100644
--- a/src/core/loader/ncch.cpp
+++ b/src/core/loader/ncch.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include <memory>
@@ -315,4 +315,8 @@ ResultStatus AppLoader_NCCH::ReadRomFS(std::vector<u8>& buffer) const {
     return ResultStatus::Error;
 }
 
+u64 AppLoader_NCCH::GetProgramId() const {
+    return *reinterpret_cast<u64 const*>(&ncch_header.program_id[0]);
+}
+
 } // namespace Loader
diff --git a/src/core/loader/ncch.h b/src/core/loader/ncch.h
index 03116add8..fd9258970 100644
--- a/src/core/loader/ncch.h
+++ b/src/core/loader/ncch.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -191,6 +191,12 @@ public:
      */
     ResultStatus ReadRomFS(std::vector<u8>& buffer) const override;
 
+    /*
+     * Gets the program id from the NCCH header
+     * @return u64 Program id
+     */
+    u64 GetProgramId() const;
+
 private:
 
     /**
diff --git a/src/core/mem_map.cpp b/src/core/mem_map.cpp
index d1c44ed24..eea6c5bf1 100644
--- a/src/core/mem_map.cpp
+++ b/src/core/mem_map.cpp
@@ -1,5 +1,5 @@
- // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Copyright 2014 Citra Emulator Project
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "common/common.h"
diff --git a/src/core/mem_map.h b/src/core/mem_map.h
index 4d47fc171..a2ef9d3af 100644
--- a/src/core/mem_map.h
+++ b/src/core/mem_map.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/core/mem_map_funcs.cpp b/src/core/mem_map_funcs.cpp
index d8720990e..fdf382ed6 100644
--- a/src/core/mem_map_funcs.cpp
+++ b/src/core/mem_map_funcs.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include <map>
diff --git a/src/core/settings.cpp b/src/core/settings.cpp
index c486f6274..8a14f75aa 100644
--- a/src/core/settings.cpp
+++ b/src/core/settings.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "settings.h"
diff --git a/src/core/settings.h b/src/core/settings.h
index 138ffc615..4b8928847 100644
--- a/src/core/settings.h
+++ b/src/core/settings.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -31,6 +31,7 @@ struct Values {
     // Core
     int cpu_core;
     int gpu_refresh_rate;
+    int frame_skip;
 
     // Data Storage
     bool use_virtual_sd;
diff --git a/src/core/system.cpp b/src/core/system.cpp
index 43d0eef2c..d6188f055 100644
--- a/src/core/system.cpp
+++ b/src/core/system.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "core/core.h"
@@ -23,10 +23,10 @@ void Init(EmuWindow* emu_window) {
     Core::Init();
     Memory::Init();
     HW::Init();
+    Kernel::Init();
     HLE::Init();
     CoreTiming::Init();
     VideoCore::Init(emu_window);
-    Kernel::Init();
 }
 
 void RunLoopFor(int cycles) {
@@ -37,13 +37,13 @@ void RunLoopUntil(u64 global_cycles) {
 }
 
 void Shutdown() {
-    Core::Shutdown();
-    Memory::Shutdown();
-    HW::Shutdown();
-    HLE::Shutdown();
-    CoreTiming::Shutdown();
     VideoCore::Shutdown();
+    CoreTiming::Shutdown();
+    HLE::Shutdown();
     Kernel::Shutdown();
+    HW::Shutdown();
+    Memory::Shutdown();
+    Core::Shutdown();
 }
 
 } // namespace
diff --git a/src/core/system.h b/src/core/system.h
index 2bc2edc75..05d836530 100644
--- a/src/core/system.h
+++ b/src/core/system.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/video_core/clipper.cpp b/src/video_core/clipper.cpp
index 632fb959a..1744066ba 100644
--- a/src/video_core/clipper.cpp
+++ b/src/video_core/clipper.cpp
@@ -1,8 +1,8 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
-#include <vector>
+#include <boost/container/static_vector.hpp>
 
 #include "clipper.h"
 #include "pica.h"
@@ -91,25 +91,31 @@ static void InitScreenCoordinates(OutputVertex& vtx)
     viewport.zscale     = float24::FromRawFloat24(registers.viewport_depth_range);
     viewport.offset_z   = float24::FromRawFloat24(registers.viewport_depth_far_plane);
 
+    float24 inv_w = float24::FromFloat32(1.f) / vtx.pos.w;
+    vtx.color *= inv_w;
+    vtx.tc0 *= inv_w;
+    vtx.tc1 *= inv_w;
+    vtx.tc2 *= inv_w;
+    vtx.pos.w = inv_w;
+
     // TODO: Not sure why the viewport width needs to be divided by 2 but the viewport height does not
-    vtx.screenpos[0] = (vtx.pos.x / vtx.pos.w + float24::FromFloat32(1.0)) * viewport.halfsize_x + viewport.offset_x;
-    vtx.screenpos[1] = (vtx.pos.y / vtx.pos.w + float24::FromFloat32(1.0)) * viewport.halfsize_y + viewport.offset_y;
-    vtx.screenpos[2] = viewport.offset_z - vtx.pos.z / vtx.pos.w * viewport.zscale;
+    vtx.screenpos[0] = (vtx.pos.x * inv_w + float24::FromFloat32(1.0)) * viewport.halfsize_x + viewport.offset_x;
+    vtx.screenpos[1] = (vtx.pos.y * inv_w + float24::FromFloat32(1.0)) * viewport.halfsize_y + viewport.offset_y;
+    vtx.screenpos[2] = viewport.offset_z - vtx.pos.z * inv_w * viewport.zscale;
 }
 
 void ProcessTriangle(OutputVertex &v0, OutputVertex &v1, OutputVertex &v2) {
-
-    // TODO (neobrain):
-    // The list of output vertices has some fixed maximum size,
-    // however I haven't taken the time to figure out what it is exactly.
-    // For now, we hence just assume a maximal size of 1000 vertices.
-    const size_t max_vertices = 1000;
-    std::vector<OutputVertex> buffer_vertices;
-    std::vector<OutputVertex*> output_list{ &v0, &v1, &v2 };
-
-    // Make sure to reserve space for all vertices.
-    // Without this, buffer reallocation would invalidate references.
-    buffer_vertices.reserve(max_vertices);
+    using boost::container::static_vector;
+
+    // Clipping a planar n-gon against a plane will remove at least 1 vertex and introduces 2 at
+    // the new edge (or less in degenerate cases). As such, we can say that each clipping plane
+    // introduces at most 1 new vertex to the polygon. Since we start with a triangle and have a
+    // fixed 6 clipping planes, the maximum number of vertices of the clipped polygon is 3 + 6 = 9.
+    static const size_t MAX_VERTICES = 9;
+    static_vector<OutputVertex, MAX_VERTICES> buffer_a = { v0, v1, v2 };
+    static_vector<OutputVertex, MAX_VERTICES> buffer_b;
+    auto* output_list = &buffer_a;
+    auto* input_list  = &buffer_b;
 
     // Simple implementation of the Sutherland-Hodgman clipping algorithm.
     // TODO: Make this less inefficient (currently lots of useless buffering overhead happens here)
@@ -120,48 +126,45 @@ void ProcessTriangle(OutputVertex &v0, OutputVertex &v1, OutputVertex &v2) {
                        ClippingEdge(ClippingEdge::POS_Z, float24::FromFloat32(+1.0)),
                        ClippingEdge(ClippingEdge::NEG_Z, float24::FromFloat32(-1.0)) }) {
 
-        const std::vector<OutputVertex*> input_list = output_list;
-        output_list.clear();
+        std::swap(input_list, output_list);
+        output_list->clear();
 
-        const OutputVertex* reference_vertex = input_list.back();
+        const OutputVertex* reference_vertex = &input_list->back();
 
-        for (const auto& vertex : input_list) {
+        for (const auto& vertex : *input_list) {
             // NOTE: This algorithm changes vertex order in some cases!
-            if (edge.IsInside(*vertex)) {
+            if (edge.IsInside(vertex)) {
                 if (edge.IsOutSide(*reference_vertex)) {
-                    buffer_vertices.push_back(edge.GetIntersection(*vertex, *reference_vertex));
-                    output_list.push_back(&(buffer_vertices.back()));
+                    output_list->push_back(edge.GetIntersection(vertex, *reference_vertex));
                 }
 
-                output_list.push_back(vertex);
+                output_list->push_back(vertex);
             } else if (edge.IsInside(*reference_vertex)) {
-                buffer_vertices.push_back(edge.GetIntersection(*vertex, *reference_vertex));
-                output_list.push_back(&(buffer_vertices.back()));
+                output_list->push_back(edge.GetIntersection(vertex, *reference_vertex));
             }
-
-            reference_vertex = vertex;
+            reference_vertex = &vertex;
         }
 
         // Need to have at least a full triangle to continue...
-        if (output_list.size() < 3)
+        if (output_list->size() < 3)
             return;
     }
 
-    InitScreenCoordinates(*(output_list[0]));
-    InitScreenCoordinates(*(output_list[1]));
+    InitScreenCoordinates((*output_list)[0]);
+    InitScreenCoordinates((*output_list)[1]);
 
-    for (size_t i = 0; i < output_list.size() - 2; i ++) {
-        OutputVertex& vtx0 = *(output_list[0]);
-        OutputVertex& vtx1 = *(output_list[i+1]);
-        OutputVertex& vtx2 = *(output_list[i+2]);
+    for (size_t i = 0; i < output_list->size() - 2; i ++) {
+        OutputVertex& vtx0 = (*output_list)[0];
+        OutputVertex& vtx1 = (*output_list)[i+1];
+        OutputVertex& vtx2 = (*output_list)[i+2];
 
         InitScreenCoordinates(vtx2);
 
         LOG_TRACE(Render_Software,
-                  "Triangle %lu/%lu (%lu buffer vertices) at position (%.3f, %.3f, %.3f, %.3f), "
+                  "Triangle %lu/%lu at position (%.3f, %.3f, %.3f, %.3f), "
                   "(%.3f, %.3f, %.3f, %.3f), (%.3f, %.3f, %.3f, %.3f) and "
                   "screen position (%.2f, %.2f, %.2f), (%.2f, %.2f, %.2f), (%.2f, %.2f, %.2f)",
-                  i,output_list.size(), buffer_vertices.size(),
+                  i, output_list->size(),
                   vtx0.pos.x.ToFloat32(), vtx0.pos.y.ToFloat32(), vtx0.pos.z.ToFloat32(), vtx0.pos.w.ToFloat32(),
                   vtx1.pos.x.ToFloat32(), vtx1.pos.y.ToFloat32(), vtx1.pos.z.ToFloat32(), vtx1.pos.w.ToFloat32(),
                   vtx2.pos.x.ToFloat32(), vtx2.pos.y.ToFloat32(), vtx2.pos.z.ToFloat32(), vtx2.pos.w.ToFloat32(),
diff --git a/src/video_core/clipper.h b/src/video_core/clipper.h
index 14d31ca1e..19ce8e140 100644
--- a/src/video_core/clipper.h
+++ b/src/video_core/clipper.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/video_core/command_processor.cpp b/src/video_core/command_processor.cpp
index b74cd3261..9602779f4 100644
--- a/src/video_core/command_processor.cpp
+++ b/src/video_core/command_processor.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "clipper.h"
@@ -9,6 +9,7 @@
 #include "primitive_assembly.h"
 #include "vertex_shader.h"
 #include "core/hle/service/gsp_gpu.h"
+#include "core/hw/gpu.h"
 
 #include "debug_utils/debug_utils.h"
 
@@ -31,6 +32,10 @@ static inline void WritePicaReg(u32 id, u32 value, u32 mask) {
     if (id >= registers.NumIds())
         return;
 
+    // If we're skipping this frame, only allow trigger IRQ
+    if (GPU::g_skip_frame && id != PICA_REG_INDEX(trigger_irq))
+        return;
+
     // TODO: Figure out how register masking acts on e.g. vs_uniform_setup.set_value
     u32 old_value = registers[id];
     registers[id] = (old_value & ~mask) | (value & mask);
@@ -56,10 +61,11 @@ static inline void WritePicaReg(u32 id, u32 value, u32 mask) {
                 g_debug_context->OnEvent(DebugContext::Event::IncomingPrimitiveBatch, nullptr);
 
             const auto& attribute_config = registers.vertex_attributes;
-            const u8* const base_address = Memory::GetPointer(attribute_config.GetBaseAddress());
+            const u32 base_address = attribute_config.GetPhysicalBaseAddress();
 
             // Information about internal vertex attributes
-            const u8* vertex_attribute_sources[16];
+            u32 vertex_attribute_sources[16];
+            std::fill(vertex_attribute_sources, &vertex_attribute_sources[16], 0xdeadbeef);
             u32 vertex_attribute_strides[16];
             u32 vertex_attribute_formats[16];
             u32 vertex_attribute_elements[16];
@@ -69,7 +75,7 @@ static inline void WritePicaReg(u32 id, u32 value, u32 mask) {
             for (int loader = 0; loader < 12; ++loader) {
                 const auto& loader_config = attribute_config.attribute_loaders[loader];
 
-                const u8* load_address = base_address + loader_config.data_offset;
+                u32 load_address = base_address + loader_config.data_offset;
 
                 // TODO: What happens if a loader overwrites a previous one's data?
                 for (unsigned component = 0; component < loader_config.component_count; ++component) {
@@ -87,9 +93,9 @@ static inline void WritePicaReg(u32 id, u32 value, u32 mask) {
             bool is_indexed = (id == PICA_REG_INDEX(trigger_draw_indexed));
 
             const auto& index_info = registers.index_array;
-            const u8* index_address_8 = (u8*)base_address + index_info.offset;
+            const u8* index_address_8 = Memory::GetPointer(PAddrToVAddr(base_address + index_info.offset));
             const u16* index_address_16 = (u16*)index_address_8;
-            bool index_u16 = (bool)index_info.format;
+            bool index_u16 = index_info.format != 0;
 
             DebugUtils::GeometryDumper geometry_dumper;
             PrimitiveAssembler<VertexShader::OutputVertex> clipper_primitive_assembler(registers.triangle_topology.Value());
@@ -108,7 +114,14 @@ static inline void WritePicaReg(u32 id, u32 value, u32 mask) {
 
                 for (int i = 0; i < attribute_config.GetNumTotalAttributes(); ++i) {
                     for (unsigned int comp = 0; comp < vertex_attribute_elements[i]; ++comp) {
-                        const u8* srcdata = vertex_attribute_sources[i] + vertex_attribute_strides[i] * vertex + comp * vertex_attribute_element_size[i];
+                        const u8* srcdata = Memory::GetPointer(PAddrToVAddr(vertex_attribute_sources[i] + vertex_attribute_strides[i] * vertex + comp * vertex_attribute_element_size[i]));
+
+                        // TODO(neobrain): Ocarina of Time 3D has GetNumTotalAttributes return 8,
+                        // yet only provides 2 valid source data addresses. Need to figure out
+                        // what's wrong there, until then we just continue when address lookup fails
+                        if (srcdata == nullptr)
+                            continue;
+
                         const float srcval = (vertex_attribute_formats[i] == 0) ? *(s8*)srcdata :
                                              (vertex_attribute_formats[i] == 1) ? *(u8*)srcdata :
                                              (vertex_attribute_formats[i] == 2) ? *(s16*)srcdata :
@@ -116,13 +129,16 @@ static inline void WritePicaReg(u32 id, u32 value, u32 mask) {
                         input.attr[i][comp] = float24::FromFloat32(srcval);
                         LOG_TRACE(HW_GPU, "Loaded component %x of attribute %x for vertex %x (index %x) from 0x%08x + 0x%08lx + 0x%04lx: %f",
                                   comp, i, vertex, index,
-                                  attribute_config.GetBaseAddress(),
+                                  attribute_config.GetPhysicalBaseAddress(),
                                   vertex_attribute_sources[i] - base_address,
-                                  srcdata - vertex_attribute_sources[i],
+                                  vertex_attribute_strides[i] * vertex + comp * vertex_attribute_element_size[i],
                                   input.attr[i][comp].ToFloat32());
                     }
                 }
 
+                if (g_debug_context)
+                    g_debug_context->OnEvent(DebugContext::Event::VertexLoaded, (void*)&input);
+
                 // NOTE: When dumping geometry, we simply assume that the first input attribute
                 //       corresponds to the position for now.
                 DebugUtils::GeometryDumper::Vertex dumped_vertex = {
@@ -151,6 +167,12 @@ static inline void WritePicaReg(u32 id, u32 value, u32 mask) {
             break;
         }
 
+        case PICA_REG_INDEX(vs_bool_uniforms):
+            for (unsigned i = 0; i < 16; ++i)
+                VertexShader::GetBoolUniform(i) = (registers.vs_bool_uniforms.Value() & (1 << i)) != 0;
+
+            break;
+
         case PICA_REG_INDEX_WORKAROUND(vs_uniform_setup.set_value[0], 0x2c1):
         case PICA_REG_INDEX_WORKAROUND(vs_uniform_setup.set_value[1], 0x2c2):
         case PICA_REG_INDEX_WORKAROUND(vs_uniform_setup.set_value[2], 0x2c3):
diff --git a/src/video_core/command_processor.h b/src/video_core/command_processor.h
index 955f9daec..bb3d4150f 100644
--- a/src/video_core/command_processor.h
+++ b/src/video_core/command_processor.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/video_core/debug_utils/debug_utils.cpp b/src/video_core/debug_utils/debug_utils.cpp
index 1a20f19ec..5921185a6 100644
--- a/src/video_core/debug_utils/debug_utils.cpp
+++ b/src/video_core/debug_utils/debug_utils.cpp
@@ -14,6 +14,8 @@
 #include <png.h>
 #endif
 
+#include <nihstro/shader_binary.h>
+
 #include "common/log.h"
 #include "common/file_util.h"
 
@@ -22,6 +24,10 @@
 
 #include "debug_utils.h"
 
+using nihstro::DVLBHeader;
+using nihstro::DVLEHeader;
+using nihstro::DVLPHeader;
+
 namespace Pica {
 
 void DebugContext::OnEvent(Event event, void* data) {
@@ -98,65 +104,6 @@ void GeometryDumper::Dump() {
     }
 }
 
-#pragma pack(1)
-struct DVLBHeader {
-    enum : u32 {
-        MAGIC_WORD = 0x424C5644, // "DVLB"
-    };
-
-    u32 magic_word;
-    u32 num_programs;
-//    u32 dvle_offset_table[];
-};
-static_assert(sizeof(DVLBHeader) == 0x8, "Incorrect structure size");
-
-struct DVLPHeader {
-    enum : u32 {
-        MAGIC_WORD = 0x504C5644, // "DVLP"
-    };
-
-    u32 magic_word;
-    u32 version;
-    u32 binary_offset;  // relative to DVLP start
-    u32 binary_size_words;
-    u32 swizzle_patterns_offset;
-    u32 swizzle_patterns_num_entries;
-    u32 unk2;
-};
-static_assert(sizeof(DVLPHeader) == 0x1C, "Incorrect structure size");
-
-struct DVLEHeader {
-    enum : u32 {
-        MAGIC_WORD = 0x454c5644, // "DVLE"
-    };
-
-    enum class ShaderType : u8 {
-        VERTEX = 0,
-        GEOMETRY = 1,
-    };
-
-    u32 magic_word;
-    u16 pad1;
-    ShaderType type;
-    u8 pad2;
-    u32 main_offset_words; // offset within binary blob
-    u32 endmain_offset_words;
-    u32 pad3;
-    u32 pad4;
-    u32 constant_table_offset;
-    u32 constant_table_size; // number of entries
-    u32 label_table_offset;
-    u32 label_table_size;
-    u32 output_register_table_offset;
-    u32 output_register_table_size;
-    u32 uniform_table_offset;
-    u32 uniform_table_size;
-    u32 symbol_table_offset;
-    u32 symbol_table_size;
-
-};
-static_assert(sizeof(DVLEHeader) == 0x40, "Incorrect structure size");
-#pragma pack()
 
 void DumpShader(const u32* binary_data, u32 binary_size, const u32* swizzle_data, u32 swizzle_size,
                 u32 main_offset, const Regs::VSOutputAttributes* output_attributes)
@@ -276,8 +223,8 @@ void DumpShader(const u32* binary_data, u32 binary_size, const u32* swizzle_data
     dvlp.binary_size_words = binary_size;
     QueueForWriting((u8*)binary_data, binary_size * sizeof(u32));
 
-    dvlp.swizzle_patterns_offset = write_offset - dvlp_offset;
-    dvlp.swizzle_patterns_num_entries = swizzle_size;
+    dvlp.swizzle_info_offset = write_offset - dvlp_offset;
+    dvlp.swizzle_info_num_entries = swizzle_size;
     u32 dummy = 0;
     for (unsigned int i = 0; i < swizzle_size; ++i) {
         QueueForWriting((u8*)&swizzle_data[i], sizeof(swizzle_data[i]));
@@ -356,39 +303,170 @@ std::unique_ptr<PicaTrace> FinishPicaTracing()
     return std::move(ret);
 }
 
-const Math::Vec4<u8> LookupTexture(const u8* source, int x, int y, const TextureInfo& info) {
-    _dbg_assert_(Debug_GPU, info.format == Pica::Regs::TextureFormat::RGB8);
+const Math::Vec4<u8> LookupTexture(const u8* source, int x, int y, const TextureInfo& info, bool disable_alpha) {
+    // Images are split into 8x8 tiles. Each tile is composed of four 4x4 subtiles each
+    // of which is composed of four 2x2 subtiles each of which is composed of four texels.
+    // Each structure is embedded into the next-bigger one in a diagonal pattern, e.g.
+    // texels are laid out in a 2x2 subtile like this:
+    // 2 3
+    // 0 1
+    //
+    // The full 8x8 tile has the texels arranged like this:
+    //
+    // 42 43 46 47 58 59 62 63
+    // 40 41 44 45 56 57 60 61
+    // 34 35 38 39 50 51 54 55
+    // 32 33 36 37 48 49 52 53
+    // 10 11 14 15 26 27 30 31
+    // 08 09 12 13 24 25 28 29
+    // 02 03 06 07 18 19 22 23
+    // 00 01 04 05 16 17 20 21
+
+    const unsigned int block_width = 8;
+    const unsigned int block_height = 8;
+
+    const unsigned int coarse_x = x & ~7;
+    const unsigned int coarse_y = y & ~7;
+
+    // Interleave the lower 3 bits of each coordinate to get the intra-block offsets, which are
+    // arranged in a Z-order curve. More details on the bit manipulation at:
+    // https://fgiesen.wordpress.com/2009/12/13/decoding-morton-codes/
+    unsigned int i = (x | (y << 8)) & 0x0707; // ---- -210
+    i = (i ^ (i << 2)) & 0x1313;              // ---2 --10
+    i = (i ^ (i << 1)) & 0x1515;              // ---2 -1-0
+    i = (i | (i >> 7)) & 0x3F;
+
+    source += coarse_y * info.stride;
+    const unsigned int offset = coarse_x * block_height + i;
+
+    switch (info.format) {
+    case Regs::TextureFormat::RGBA8:
+    {
+        const u8* source_ptr = source + offset * 4;
+        return { source_ptr[3], source_ptr[2], source_ptr[1], disable_alpha ? (u8)255 : source_ptr[0] };
+    }
+
+    case Regs::TextureFormat::RGB8:
+    {
+        const u8* source_ptr = source + offset * 3;
+        return { source_ptr[2], source_ptr[1], source_ptr[0], 255 };
+    }
+
+    case Regs::TextureFormat::RGBA5551:
+    {
+        const u16 source_ptr = *(const u16*)(source + offset * 2);
+        u8 r = (source_ptr >> 11) & 0x1F;
+        u8 g = ((source_ptr) >> 6) & 0x1F;
+        u8 b = (source_ptr >> 1) & 0x1F;
+        u8 a = source_ptr & 1;
+        return Math::MakeVec<u8>((r << 3) | (r >> 2), (g << 3) | (g >> 2), (b << 3) | (b >> 2), disable_alpha ? 255 : (a * 255));
+    }
+
+    case Regs::TextureFormat::RGB565:
+    {
+        const u16 source_ptr = *(const u16*)(source + offset * 2);
+        u8 r = (source_ptr >> 11) & 0x1F;
+        u8 g = ((source_ptr) >> 5) & 0x3F;
+        u8 b = (source_ptr) & 0x1F;
+        return Math::MakeVec<u8>((r << 3) | (r >> 2), (g << 2) | (g >> 4), (b << 3) | (b >> 2), 255);
+    }
+
+    case Regs::TextureFormat::RGBA4:
+    {
+        const u8* source_ptr = source + offset * 2;
+        u8 r = source_ptr[1] >> 4;
+        u8 g = source_ptr[1] & 0xFF;
+        u8 b = source_ptr[0] >> 4;
+        u8 a = source_ptr[0] & 0xFF;
+        r = (r << 4) | r;
+        g = (g << 4) | g;
+        b = (b << 4) | b;
+        a = (a << 4) | a;
+        return { r, g, b, disable_alpha ? (u8)255 : a };
+    }
+
+    case Regs::TextureFormat::IA8:
+    {
+        const u8* source_ptr = source + offset * 2;
 
-    // Cf. rasterizer code for an explanation of this algorithm.
-    int texel_index_within_tile = 0;
-    for (int block_size_index = 0; block_size_index < 3; ++block_size_index) {
-        int sub_tile_width = 1 << block_size_index;
-        int sub_tile_height = 1 << block_size_index;
+        // TODO: component order not verified
 
-        int sub_tile_index = (x & sub_tile_width) << block_size_index;
-        sub_tile_index += 2 * ((y & sub_tile_height) << block_size_index);
-        texel_index_within_tile += sub_tile_index;
+        if (disable_alpha) {
+            // Show intensity as red, alpha as green
+            return { source_ptr[0], source_ptr[1], 0, 255 };
+        } else {
+            return { source_ptr[0], source_ptr[0], source_ptr[0], source_ptr[1]};
+        }
     }
 
-    const int block_width = 8;
-    const int block_height = 8;
+    case Regs::TextureFormat::I8:
+    {
+        const u8* source_ptr = source + offset;
+        return { *source_ptr, *source_ptr, *source_ptr, 255 };
+    }
+
+    case Regs::TextureFormat::A8:
+    {
+        const u8* source_ptr = source + offset;
+
+        if (disable_alpha) {
+            return { *source_ptr, *source_ptr, *source_ptr, 255 };
+        } else {
+            return { 0, 0, 0, *source_ptr };
+        }
+    }
+
+    case Regs::TextureFormat::IA4:
+    {
+        const u8* source_ptr = source + offset / 2;
+
+        // TODO: component order not verified
 
-    int coarse_x = (x / block_width) * block_width;
-    int coarse_y = (y / block_height) * block_height;
+        u8 i = (*source_ptr) & 0xF;
+        u8 a = ((*source_ptr) & 0xF0) >> 4;
+        a |= a << 4;
+        i |= i << 4;
 
-    const u8* source_ptr = source + coarse_x * block_height * 3 + coarse_y * info.stride + texel_index_within_tile * 3;
-    return { source_ptr[2], source_ptr[1], source_ptr[0], 255 };
+        if (disable_alpha) {
+            // Show intensity as red, alpha as green
+            return { i, a, 0, 255 };
+        } else {
+            return { i, i, i, a };
+        }
+    }
+
+    case Regs::TextureFormat::A4:
+    {
+        const u8* source_ptr = source + offset / 2;
+
+        // TODO: component order not verified
+
+        u8 a = (coarse_x % 2) ? ((*source_ptr)&0xF) : (((*source_ptr) & 0xF0) >> 4);
+        a |= a << 4;
+
+        if (disable_alpha) {
+            return { *source_ptr, *source_ptr, *source_ptr, 255 };
+        } else {
+            return { 0, 0, 0, *source_ptr };
+        }
+    }
+
+    default:
+        LOG_ERROR(HW_GPU, "Unknown texture format: %x", (u32)info.format);
+        _dbg_assert_(HW_GPU, 0);
+        return {};
+    }
 }
 
 TextureInfo TextureInfo::FromPicaRegister(const Regs::TextureConfig& config,
                                           const Regs::TextureFormat& format)
 {
     TextureInfo info;
-    info.address = config.GetPhysicalAddress();
+    info.physical_address = config.GetPhysicalAddress();
     info.width = config.width;
     info.height = config.height;
     info.format = format;
-    info.stride = Pica::Regs::BytesPerPixel(info.format) * info.width;
+    info.stride = Pica::Regs::NibblesPerPixel(info.format) * info.width / 2;
     return info;
 }
 
@@ -499,26 +577,32 @@ void DumpTevStageConfig(const std::array<Pica::Regs::TevStageConfig,6>& stages)
     for (size_t index = 0; index < stages.size(); ++index) {
         const auto& tev_stage = stages[index];
 
-        const std::map<Source, std::string> source_map = {
+        static const std::map<Source, std::string> source_map = {
             { Source::PrimaryColor, "PrimaryColor" },
             { Source::Texture0, "Texture0" },
+            { Source::Texture1, "Texture1" },
+            { Source::Texture2, "Texture2" },
             { Source::Constant, "Constant" },
             { Source::Previous, "Previous" },
         };
 
-        const std::map<ColorModifier, std::string> color_modifier_map = {
-            { ColorModifier::SourceColor, { "%source.rgb" } }
+        static const std::map<ColorModifier, std::string> color_modifier_map = {
+            { ColorModifier::SourceColor, { "%source.rgb" } },
+            { ColorModifier::SourceAlpha, { "%source.aaa" } },
         };
-        const std::map<AlphaModifier, std::string> alpha_modifier_map = {
-            { AlphaModifier::SourceAlpha, "%source.a" }
+        static const std::map<AlphaModifier, std::string> alpha_modifier_map = {
+            { AlphaModifier::SourceAlpha, "%source.a" },
+            { AlphaModifier::OneMinusSourceAlpha, "(255 - %source.a)" },
         };
 
-        std::map<Operation, std::string> combiner_map = {
+        static const std::map<Operation, std::string> combiner_map = {
             { Operation::Replace, "%source1" },
             { Operation::Modulate, "(%source1 * %source2) / 255" },
+            { Operation::Add, "(%source1 + %source2)" },
+            { Operation::Lerp, "lerp(%source1, %source2, %source3)" },
         };
 
-        auto ReplacePattern =
+        static auto ReplacePattern =
                 [](const std::string& input, const std::string& pattern, const std::string& replacement) -> std::string {
                     size_t start = input.find(pattern);
                     if (start == std::string::npos)
@@ -528,8 +612,8 @@ void DumpTevStageConfig(const std::array<Pica::Regs::TevStageConfig,6>& stages)
                     ret.replace(start, pattern.length(), replacement);
                     return ret;
                 };
-        auto GetColorSourceStr =
-                [&source_map,&color_modifier_map,&ReplacePattern](const Source& src, const ColorModifier& modifier) {
+        static auto GetColorSourceStr =
+                [](const Source& src, const ColorModifier& modifier) {
                     auto src_it = source_map.find(src);
                     std::string src_str = "Unknown";
                     if (src_it != source_map.end())
@@ -542,8 +626,8 @@ void DumpTevStageConfig(const std::array<Pica::Regs::TevStageConfig,6>& stages)
 
                     return ReplacePattern(modifier_str, "%source", src_str);
                 };
-        auto GetColorCombinerStr =
-                [&](const Regs::TevStageConfig& tev_stage) {
+        static auto GetColorCombinerStr =
+                [](const Regs::TevStageConfig& tev_stage) {
                     auto op_it = combiner_map.find(tev_stage.color_op);
                     std::string op_str = "Unknown op (%source1, %source2, %source3)";
                     if (op_it != combiner_map.end())
@@ -553,8 +637,8 @@ void DumpTevStageConfig(const std::array<Pica::Regs::TevStageConfig,6>& stages)
                     op_str = ReplacePattern(op_str, "%source2", GetColorSourceStr(tev_stage.color_source2, tev_stage.color_modifier2));
                     return   ReplacePattern(op_str, "%source3", GetColorSourceStr(tev_stage.color_source3, tev_stage.color_modifier3));
                 };
-        auto GetAlphaSourceStr =
-                [&source_map,&alpha_modifier_map,&ReplacePattern](const Source& src, const AlphaModifier& modifier) {
+        static auto GetAlphaSourceStr =
+                [](const Source& src, const AlphaModifier& modifier) {
                     auto src_it = source_map.find(src);
                     std::string src_str = "Unknown";
                     if (src_it != source_map.end())
@@ -567,8 +651,8 @@ void DumpTevStageConfig(const std::array<Pica::Regs::TevStageConfig,6>& stages)
 
                     return ReplacePattern(modifier_str, "%source", src_str);
                 };
-        auto GetAlphaCombinerStr =
-                [&](const Regs::TevStageConfig& tev_stage) {
+        static auto GetAlphaCombinerStr =
+                [](const Regs::TevStageConfig& tev_stage) {
                     auto op_it = combiner_map.find(tev_stage.alpha_op);
                     std::string op_str = "Unknown op (%source1, %source2, %source3)";
                     if (op_it != combiner_map.end())
diff --git a/src/video_core/debug_utils/debug_utils.h b/src/video_core/debug_utils/debug_utils.h
index 51f14f12f..f361a5385 100644
--- a/src/video_core/debug_utils/debug_utils.h
+++ b/src/video_core/debug_utils/debug_utils.h
@@ -26,6 +26,7 @@ public:
         CommandProcessed,
         IncomingPrimitiveBatch,
         FinishedPrimitiveBatch,
+        VertexLoaded,
 
         NumEvents
     };
@@ -192,7 +193,7 @@ void OnPicaRegWrite(u32 id, u32 value);
 std::unique_ptr<PicaTrace> FinishPicaTracing();
 
 struct TextureInfo {
-    unsigned int address;
+    PAddr physical_address;
     int width;
     int height;
     int stride;
@@ -202,7 +203,17 @@ struct TextureInfo {
                                         const Pica::Regs::TextureFormat& format);
 };
 
-const Math::Vec4<u8> LookupTexture(const u8* source, int x, int y, const TextureInfo& info);
+/**
+ * Lookup texel located at the given coordinates and return an RGBA vector of its color.
+ * @param source Source pointer to read data from
+ * @param s,t Texture coordinates to read from
+ * @param info TextureInfo object describing the texture setup
+ * @param disable_alpha This is used for debug widgets which use this method to display textures without providing a good way to visualize alpha by themselves. If true, this will return 255 for the alpha component, and either drop the information entirely or store it in an "unused" color channel.
+ * @todo Eventually we should get rid of the disable_alpha parameter.
+ */
+const Math::Vec4<u8> LookupTexture(const u8* source, int s, int t, const TextureInfo& info,
+                                   bool disable_alpha = false);
+
 void DumpTexture(const Pica::Regs::TextureConfig& texture_config, u8* data);
 
 void DumpTevStageConfig(const std::array<Pica::Regs::TevStageConfig,6>& stages);
diff --git a/src/video_core/gpu_debugger.h b/src/video_core/gpu_debugger.h
index 16b1656bb..a51d49c92 100644
--- a/src/video_core/gpu_debugger.h
+++ b/src/video_core/gpu_debugger.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -85,7 +85,7 @@ public:
 
     void UnregisterObserver(DebuggerObserver* observer)
     {
-        std::remove(observers.begin(), observers.end(), observer);
+        observers.erase(std::remove(observers.begin(), observers.end(), observer), observers.end());
         observer->observed = nullptr;
     }
 
diff --git a/src/video_core/math.h b/src/video_core/math.h
index 83ba81235..9622e7614 100644
--- a/src/video_core/math.h
+++ b/src/video_core/math.h
@@ -1,4 +1,4 @@
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 
diff --git a/src/video_core/pica.h b/src/video_core/pica.h
index 4c3791ad9..38bac748c 100644
--- a/src/video_core/pica.h
+++ b/src/video_core/pica.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -8,6 +8,7 @@
 #include <cstddef>
 #include <initializer_list>
 #include <map>
+#include <vector>
 
 #include "common/bit_field.h"
 #include "common/common_types.h"
@@ -104,6 +105,11 @@ struct Regs {
     INSERT_PADDING_WORDS(0x17);
 
     struct TextureConfig {
+        enum WrapMode : u32 {
+            ClampToEdge = 0,
+            Repeat      = 2,
+        };
+
         INSERT_PADDING_WORDS(0x1);
 
         union {
@@ -111,12 +117,17 @@ struct Regs {
             BitField<16, 16, u32> width;
         };
 
-        INSERT_PADDING_WORDS(0x2);
+        union {
+            BitField< 8, 2, WrapMode> wrap_s;
+            BitField<11, 2, WrapMode> wrap_t;
+        };
+
+        INSERT_PADDING_WORDS(0x1);
 
         u32 address;
 
         u32 GetPhysicalAddress() const {
-            return DecodeAddressRegister(address) - Memory::FCRAM_PADDR + Memory::HEAP_LINEAR_VADDR;
+            return DecodeAddressRegister(address);
         }
 
         // texture1 and texture2 store the texture format directly after the address
@@ -131,36 +142,70 @@ struct Regs {
         RGBA5551     =  2,
         RGB565       =  3,
         RGBA4        =  4,
+        IA8          =  5,
+
+        I8           =  7,
+        A8           =  8,
+        IA4          =  9,
 
+        A4           = 11,
         // TODO: Support for the other formats is not implemented, yet.
         // Seems like they are luminance formats and compressed textures.
     };
 
-    static unsigned BytesPerPixel(TextureFormat format) {
+    static unsigned NibblesPerPixel(TextureFormat format) {
         switch (format) {
         case TextureFormat::RGBA8:
-            return 4;
+            return 8;
 
         case TextureFormat::RGB8:
-            return 3;
+            return 6;
 
         case TextureFormat::RGBA5551:
         case TextureFormat::RGB565:
         case TextureFormat::RGBA4:
-            return 2;
+        case TextureFormat::IA8:
+            return 4;
 
-        default:
-            // placeholder for yet unknown formats
+        case TextureFormat::A4:
             return 1;
+
+        case TextureFormat::I8:
+        case TextureFormat::A8:
+        case TextureFormat::IA4:
+        default:  // placeholder for yet unknown formats
+            return 2;
         }
     }
 
-    BitField< 0, 1, u32> texturing_enable;
+    union {
+        BitField< 0, 1, u32> texture0_enable;
+        BitField< 1, 1, u32> texture1_enable;
+        BitField< 2, 1, u32> texture2_enable;
+    };
     TextureConfig texture0;
     INSERT_PADDING_WORDS(0x8);
     BitField<0, 4, TextureFormat> texture0_format;
-
-    INSERT_PADDING_WORDS(0x31);
+    INSERT_PADDING_WORDS(0x2);
+    TextureConfig texture1;
+    BitField<0, 4, TextureFormat> texture1_format;
+    INSERT_PADDING_WORDS(0x2);
+    TextureConfig texture2;
+    BitField<0, 4, TextureFormat> texture2_format;
+    INSERT_PADDING_WORDS(0x21);
+
+    struct FullTextureConfig {
+        const bool enabled;
+        const TextureConfig config;
+        const TextureFormat format;
+    };
+    const std::array<FullTextureConfig, 3> GetTextures() const {
+        return {{
+                   { texture0_enable.ToBool(), texture0, texture0_format },
+                   { texture1_enable.ToBool(), texture1, texture1_format },
+                   { texture2_enable.ToBool(), texture2, texture2_format }
+               }};
+    }
 
     // 0xc0-0xff: Texture Combiner (akin to glTexEnv)
     struct TevStageConfig {
@@ -282,11 +327,11 @@ struct Regs {
 
         INSERT_PADDING_WORDS(0x1);
 
-        inline u32 GetColorBufferAddress() const {
-            return Memory::PhysicalToVirtualAddress(DecodeAddressRegister(color_buffer_address));
+        inline u32 GetColorBufferPhysicalAddress() const {
+            return DecodeAddressRegister(color_buffer_address);
         }
-        inline u32 GetDepthBufferAddress() const {
-            return Memory::PhysicalToVirtualAddress(DecodeAddressRegister(depth_buffer_address));
+        inline u32 GetDepthBufferPhysicalAddress() const {
+            return DecodeAddressRegister(depth_buffer_address);
         }
 
         inline u32 GetWidth() const {
@@ -310,9 +355,8 @@ struct Regs {
 
         BitField<0, 29, u32> base_address;
 
-        inline u32 GetBaseAddress() const {
-            // TODO: Ugly, should fix PhysicalToVirtualAddress instead
-            return DecodeAddressRegister(base_address) - Memory::FCRAM_PADDR + Memory::HEAP_LINEAR_VADDR;
+        u32 GetPhysicalBaseAddress() const {
+            return DecodeAddressRegister(base_address);
         }
 
         // Descriptor for internal vertex attributes
@@ -448,7 +492,11 @@ struct Regs {
 
     BitField<8, 2, TriangleTopology> triangle_topology;
 
-    INSERT_PADDING_WORDS(0x5b);
+    INSERT_PADDING_WORDS(0x51);
+
+    BitField<0, 16, u32> vs_bool_uniforms;
+
+    INSERT_PADDING_WORDS(0x9);
 
     // Offset to shader program entry point (in words)
     BitField<0, 16, u32> vs_main_offset;
@@ -542,11 +590,11 @@ struct Regs {
     static std::string GetCommandName(int index) {
         std::map<u32, std::string> map;
 
-        Regs regs;
         #define ADD_FIELD(name)                                                                               \
             do {                                                                                              \
                 map.insert({PICA_REG_INDEX(name), #name});                                                    \
-                for (u32 i = PICA_REG_INDEX(name) + 1; i < PICA_REG_INDEX(name) + sizeof(regs.name) / 4; ++i) \
+                /* TODO: change to Regs::name when VS2015 and other compilers support it  */                   \
+                for (u32 i = PICA_REG_INDEX(name) + 1; i < PICA_REG_INDEX(name) + sizeof(Regs().name) / 4; ++i) \
                     map.insert({i, #name + std::string("+") + std::to_string(i-PICA_REG_INDEX(name))});       \
             } while(false)
 
@@ -556,9 +604,13 @@ struct Regs {
         ADD_FIELD(viewport_depth_range);
         ADD_FIELD(viewport_depth_far_plane);
         ADD_FIELD(viewport_corner);
-        ADD_FIELD(texturing_enable);
+        ADD_FIELD(texture0_enable);
         ADD_FIELD(texture0);
         ADD_FIELD(texture0_format);
+        ADD_FIELD(texture1);
+        ADD_FIELD(texture1_format);
+        ADD_FIELD(texture2);
+        ADD_FIELD(texture2_format);
         ADD_FIELD(tev_stage0);
         ADD_FIELD(tev_stage1);
         ADD_FIELD(tev_stage2);
@@ -572,6 +624,7 @@ struct Regs {
         ADD_FIELD(trigger_draw);
         ADD_FIELD(trigger_draw_indexed);
         ADD_FIELD(triangle_topology);
+        ADD_FIELD(vs_bool_uniforms);
         ADD_FIELD(vs_main_offset);
         ADD_FIELD(vs_input_register_map);
         ADD_FIELD(vs_uniform_setup);
@@ -622,9 +675,13 @@ ASSERT_REG_POSITION(viewport_depth_far_plane, 0x4e);
 ASSERT_REG_POSITION(vs_output_attributes[0], 0x50);
 ASSERT_REG_POSITION(vs_output_attributes[1], 0x51);
 ASSERT_REG_POSITION(viewport_corner, 0x68);
-ASSERT_REG_POSITION(texturing_enable, 0x80);
+ASSERT_REG_POSITION(texture0_enable, 0x80);
 ASSERT_REG_POSITION(texture0, 0x81);
 ASSERT_REG_POSITION(texture0_format, 0x8e);
+ASSERT_REG_POSITION(texture1, 0x91);
+ASSERT_REG_POSITION(texture1_format, 0x96);
+ASSERT_REG_POSITION(texture2, 0x99);
+ASSERT_REG_POSITION(texture2_format, 0x9e);
 ASSERT_REG_POSITION(tev_stage0, 0xc0);
 ASSERT_REG_POSITION(tev_stage1, 0xc8);
 ASSERT_REG_POSITION(tev_stage2, 0xd0);
@@ -638,6 +695,7 @@ ASSERT_REG_POSITION(num_vertices, 0x228);
 ASSERT_REG_POSITION(trigger_draw, 0x22e);
 ASSERT_REG_POSITION(trigger_draw_indexed, 0x22f);
 ASSERT_REG_POSITION(triangle_topology, 0x25e);
+ASSERT_REG_POSITION(vs_bool_uniforms, 0x2b0);
 ASSERT_REG_POSITION(vs_main_offset, 0x2ba);
 ASSERT_REG_POSITION(vs_input_register_map, 0x2bb);
 ASSERT_REG_POSITION(vs_uniform_setup, 0x2c0);
@@ -699,6 +757,26 @@ struct float24 {
         return float24::FromFloat32(ToFloat32() - flt.ToFloat32());
     }
 
+    float24& operator *= (const float24& flt) {
+        value *= flt.ToFloat32();
+        return *this;
+    }
+
+    float24& operator /= (const float24& flt) {
+        value /= flt.ToFloat32();
+        return *this;
+    }
+
+    float24& operator += (const float24& flt) {
+        value += flt.ToFloat32();
+        return *this;
+    }
+
+    float24& operator -= (const float24& flt) {
+        value -= flt.ToFloat32();
+        return *this;
+    }
+
     float24 operator - () const {
         return float24::FromFloat32(-ToFloat32());
     }
@@ -719,6 +797,14 @@ struct float24 {
         return ToFloat32() <= flt.ToFloat32();
     }
 
+    bool operator == (const float24& flt) const {
+        return ToFloat32() == flt.ToFloat32();
+    }
+
+    bool operator != (const float24& flt) const {
+        return ToFloat32() != flt.ToFloat32();
+    }
+
 private:
     // Stored as a regular float, merely for convenience
     // TODO: Perform proper arithmetic on this!
@@ -736,5 +822,15 @@ union CommandHeader {
     BitField<31,  1, u32> group_commands;
 };
 
+// TODO: Ugly, should fix PhysicalToVirtualAddress instead
+inline static u32 PAddrToVAddr(u32 addr) {
+    if (addr >= Memory::VRAM_PADDR && addr < Memory::VRAM_PADDR + Memory::VRAM_SIZE) {
+        return addr - Memory::VRAM_PADDR + Memory::VRAM_VADDR;
+    } else if (addr >= Memory::FCRAM_PADDR && addr < Memory::FCRAM_PADDR + Memory::FCRAM_SIZE) {
+        return addr - Memory::FCRAM_PADDR + Memory::HEAP_LINEAR_VADDR;
+    } else {
+        return 0;
+    }
+}
 
 } // namespace
diff --git a/src/video_core/primitive_assembly.cpp b/src/video_core/primitive_assembly.cpp
index 102693ed9..242a07e26 100644
--- a/src/video_core/primitive_assembly.cpp
+++ b/src/video_core/primitive_assembly.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "pica.h"
@@ -30,20 +30,27 @@ void PrimitiveAssembler<VertexType>::SubmitVertex(VertexType& vtx, TriangleHandl
             }
             break;
 
+        case Regs::TriangleTopology::Strip:
         case Regs::TriangleTopology::Fan:
-            if (buffer_index == 2) {
-                buffer_index = 0;
-
-                triangle_handler(buffer[0], buffer[1], vtx);
+            if (strip_ready) {
+                // TODO: Should be "buffer[0], buffer[1], vtx" instead!
+                // Not quite sure why we need this order for things to show up properly.
+                // Maybe a bug in the rasterizer?
+                triangle_handler(buffer[1], buffer[0], vtx);
+            }
+            buffer[buffer_index] = vtx;
 
-                buffer[1] = vtx;
-            } else {
-                buffer[buffer_index++] = vtx;
+            if (topology == Regs::TriangleTopology::Strip) {
+                strip_ready |= (buffer_index == 1);
+                buffer_index = !buffer_index;
+            } else if (topology == Regs::TriangleTopology::Fan) {
+                buffer_index = 1;
+                strip_ready = true;
             }
             break;
 
         default:
-            LOG_ERROR(Render_Software, "Unknown triangle topology %x:", (int)topology);
+            LOG_ERROR(HW_GPU, "Unknown triangle topology %x:", (int)topology);
             break;
     }
 }
diff --git a/src/video_core/primitive_assembly.h b/src/video_core/primitive_assembly.h
index ea2e2f61e..52ff4cd89 100644
--- a/src/video_core/primitive_assembly.h
+++ b/src/video_core/primitive_assembly.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -37,6 +37,7 @@ private:
 
     int buffer_index;
     VertexType buffer[2];
+    bool strip_ready = false;
 };
 
 
diff --git a/src/video_core/rasterizer.cpp b/src/video_core/rasterizer.cpp
index b7e04a560..a80148872 100644
--- a/src/video_core/rasterizer.cpp
+++ b/src/video_core/rasterizer.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include <algorithm>
@@ -18,7 +18,7 @@ namespace Pica {
 namespace Rasterizer {
 
 static void DrawPixel(int x, int y, const Math::Vec4<u8>& color) {
-    u32* color_buffer = (u32*)Memory::GetPointer(registers.framebuffer.GetColorBufferAddress());
+    u32* color_buffer = reinterpret_cast<u32*>(Memory::GetPointer(PAddrToVAddr(registers.framebuffer.GetColorBufferPhysicalAddress())));
     u32 value = (color.a() << 24) | (color.r() << 16) | (color.g() << 8) | color.b();
 
     // Assuming RGBA8 format until actual framebuffer format handling is implemented
@@ -26,14 +26,14 @@ static void DrawPixel(int x, int y, const Math::Vec4<u8>& color) {
 }
 
 static u32 GetDepth(int x, int y) {
-    u16* depth_buffer = (u16*)Memory::GetPointer(registers.framebuffer.GetDepthBufferAddress());
+    u16* depth_buffer = reinterpret_cast<u16*>(Memory::GetPointer(PAddrToVAddr(registers.framebuffer.GetDepthBufferPhysicalAddress())));
 
     // Assuming 16-bit depth buffer format until actual format handling is implemented
     return *(depth_buffer + x + y * registers.framebuffer.GetWidth());
 }
 
 static void SetDepth(int x, int y, u16 value) {
-    u16* depth_buffer = (u16*)Memory::GetPointer(registers.framebuffer.GetDepthBufferAddress());
+    u16* depth_buffer = reinterpret_cast<u16*>(Memory::GetPointer(PAddrToVAddr(registers.framebuffer.GetDepthBufferPhysicalAddress())));
 
     // Assuming 16-bit depth buffer format until actual format handling is implemented
     *(depth_buffer + x + y * registers.framebuffer.GetWidth()) = value;
@@ -106,6 +106,11 @@ void ProcessTriangle(const VertexShader::OutputVertex& v0,
     int bias1 = IsRightSideOrFlatBottomEdge(vtxpos[1].xy(), vtxpos[2].xy(), vtxpos[0].xy()) ? -1 : 0;
     int bias2 = IsRightSideOrFlatBottomEdge(vtxpos[2].xy(), vtxpos[0].xy(), vtxpos[1].xy()) ? -1 : 0;
 
+    auto w_inverse = Math::MakeVec(v0.pos.w, v1.pos.w, v2.pos.w);
+
+    auto textures = registers.GetTextures();
+    auto tev_stages = registers.GetTevStages();
+
     // TODO: Not sure if looping through x first might be faster
     for (u16 y = min_y; y < max_y; y += 0x10) {
         for (u16 x = min_x; x < max_x; x += 0x10) {
@@ -129,6 +134,11 @@ void ProcessTriangle(const VertexShader::OutputVertex& v0,
             if (w0 < 0 || w1 < 0 || w2 < 0)
                 continue;
 
+            auto baricentric_coordinates = Math::MakeVec(float24::FromFloat32(static_cast<float>(w0)),
+                                                float24::FromFloat32(static_cast<float>(w1)),
+                                                float24::FromFloat32(static_cast<float>(w2)));
+            float24 interpolated_w_inverse = float24::FromFloat32(1.0f) / Math::Dot(w_inverse, baricentric_coordinates);
+
             // Perspective correct attribute interpolation:
             // Attribute values cannot be calculated by simple linear interpolation since
             // they are not linear in screen space. For example, when interpolating a
@@ -145,19 +155,9 @@ void ProcessTriangle(const VertexShader::OutputVertex& v0,
             //
             // The generalization to three vertices is straightforward in baricentric coordinates.
             auto GetInterpolatedAttribute = [&](float24 attr0, float24 attr1, float24 attr2) {
-                auto attr_over_w = Math::MakeVec(attr0 / v0.pos.w,
-                                                 attr1 / v1.pos.w,
-                                                 attr2 / v2.pos.w);
-                auto w_inverse   = Math::MakeVec(float24::FromFloat32(1.f) / v0.pos.w,
-                                                 float24::FromFloat32(1.f) / v1.pos.w,
-                                                 float24::FromFloat32(1.f) / v2.pos.w);
-                auto baricentric_coordinates = Math::MakeVec(float24::FromFloat32(static_cast<float>(w0)),
-                                                             float24::FromFloat32(static_cast<float>(w1)),
-                                                             float24::FromFloat32(static_cast<float>(w2)));
-
+                auto attr_over_w = Math::MakeVec(attr0, attr1, attr2);
                 float24 interpolated_attr_over_w = Math::Dot(attr_over_w, baricentric_coordinates);
-                float24 interpolated_w_inverse   = Math::Dot(w_inverse,   baricentric_coordinates);
-                return interpolated_attr_over_w / interpolated_w_inverse;
+                return interpolated_attr_over_w * interpolated_w_inverse;
             };
 
             Math::Vec4<u8> primary_color{
@@ -167,60 +167,48 @@ void ProcessTriangle(const VertexShader::OutputVertex& v0,
                 (u8)(GetInterpolatedAttribute(v0.color.a(), v1.color.a(), v2.color.a()).ToFloat32() * 255)
             };
 
-            Math::Vec4<u8> texture_color{};
-            float24 u = GetInterpolatedAttribute(v0.tc0.u(), v1.tc0.u(), v2.tc0.u());
-            float24 v = GetInterpolatedAttribute(v0.tc0.v(), v1.tc0.v(), v2.tc0.v());
-            if (registers.texturing_enable) {
-                // Images are split into 8x8 tiles. Each tile is composed of four 4x4 subtiles each
-                // of which is composed of four 2x2 subtiles each of which is composed of four texels.
-                // Each structure is embedded into the next-bigger one in a diagonal pattern, e.g.
-                // texels are laid out in a 2x2 subtile like this:
-                // 2 3
-                // 0 1
-                //
-                // The full 8x8 tile has the texels arranged like this:
-                //
-                // 42 43 46 47 58 59 62 63
-                // 40 41 44 45 56 57 60 61
-                // 34 35 38 39 50 51 54 55
-                // 32 33 36 37 48 49 52 53
-                // 10 11 14 15 26 27 30 31
-                // 08 09 12 13 24 25 28 29
-                // 02 03 06 07 18 19 22 23
-                // 00 01 04 05 16 17 20 21
-
-                // TODO: This is currently hardcoded for RGB8
-                u32* texture_data = (u32*)Memory::GetPointer(registers.texture0.GetPhysicalAddress());
-
-                // TODO(neobrain): Not sure if this swizzling pattern is used for all textures.
-                // To be flexible in case different but similar patterns are used, we keep this
-                // somewhat inefficient code around for now.
-                int s = (int)(u * float24::FromFloat32(static_cast<float>(registers.texture0.width))).ToFloat32();
-                int t = (int)(v * float24::FromFloat32(static_cast<float>(registers.texture0.height))).ToFloat32();
-                int texel_index_within_tile = 0;
-                for (int block_size_index = 0; block_size_index < 3; ++block_size_index) {
-                    int sub_tile_width = 1 << block_size_index;
-                    int sub_tile_height = 1 << block_size_index;
-
-                    int sub_tile_index = (s & sub_tile_width) << block_size_index;
-                    sub_tile_index += 2 * ((t & sub_tile_height) << block_size_index);
-                    texel_index_within_tile += sub_tile_index;
-                }
-
-                const int block_width = 8;
-                const int block_height = 8;
-
-                int coarse_s = (s / block_width) * block_width;
-                int coarse_t = (t / block_height) * block_height;
-
-                const int row_stride = registers.texture0.width * 3;
-                u8* source_ptr = (u8*)texture_data + coarse_s * block_height * 3 + coarse_t * row_stride + texel_index_within_tile * 3;
-                texture_color.r() = source_ptr[2];
-                texture_color.g() = source_ptr[1];
-                texture_color.b() = source_ptr[0];
-                texture_color.a() = 0xFF;
-
-                DebugUtils::DumpTexture(registers.texture0, (u8*)texture_data);
+            Math::Vec2<float24> uv[3];
+            uv[0].u() = GetInterpolatedAttribute(v0.tc0.u(), v1.tc0.u(), v2.tc0.u());
+            uv[0].v() = GetInterpolatedAttribute(v0.tc0.v(), v1.tc0.v(), v2.tc0.v());
+            uv[1].u() = GetInterpolatedAttribute(v0.tc1.u(), v1.tc1.u(), v2.tc1.u());
+            uv[1].v() = GetInterpolatedAttribute(v0.tc1.v(), v1.tc1.v(), v2.tc1.v());
+            uv[2].u() = GetInterpolatedAttribute(v0.tc2.u(), v1.tc2.u(), v2.tc2.u());
+            uv[2].v() = GetInterpolatedAttribute(v0.tc2.v(), v1.tc2.v(), v2.tc2.v());
+
+            Math::Vec4<u8> texture_color[3]{};
+            for (int i = 0; i < 3; ++i) {
+                const auto& texture = textures[i];
+                if (!texture.enabled)
+                    continue;
+
+                _dbg_assert_(HW_GPU, 0 != texture.config.address);
+
+                int s = (int)(uv[i].u() * float24::FromFloat32(static_cast<float>(texture.config.width))).ToFloat32();
+                int t = (int)(uv[i].v() * float24::FromFloat32(static_cast<float>(texture.config.height))).ToFloat32();
+                auto GetWrappedTexCoord = [](Regs::TextureConfig::WrapMode mode, int val, unsigned size) {
+                    switch (mode) {
+                        case Regs::TextureConfig::ClampToEdge:
+                            val = std::max(val, 0);
+                            val = std::min(val, (int)size - 1);
+                            return val;
+
+                        case Regs::TextureConfig::Repeat:
+                            return (int)(((unsigned)val) % size);
+
+                        default:
+                            LOG_ERROR(HW_GPU, "Unknown texture coordinate wrapping mode %x\n", (int)mode);
+                            _dbg_assert_(HW_GPU, 0);
+                            return 0;
+                    }
+                };
+                s = GetWrappedTexCoord(registers.texture0.wrap_s, s, registers.texture0.width);
+                t = GetWrappedTexCoord(registers.texture0.wrap_t, t, registers.texture0.height);
+
+                u8* texture_data = Memory::GetPointer(PAddrToVAddr(texture.config.GetPhysicalAddress()));
+                auto info = DebugUtils::TextureInfo::FromPicaRegister(texture.config, texture.format);
+
+                texture_color[i] = DebugUtils::LookupTexture(texture_data, s, t, info);
+                DebugUtils::DumpTexture(texture.config, texture_data);
             }
 
             // Texture environment - consists of 6 stages of color and alpha combining.
@@ -231,28 +219,35 @@ void ProcessTriangle(const VertexShader::OutputVertex& v0,
             // with some basic arithmetic. Alpha combiners can be configured separately but work
             // analogously.
             Math::Vec4<u8> combiner_output;
-            for (auto tev_stage : registers.GetTevStages()) {
+            for (const auto& tev_stage : tev_stages) {
                 using Source = Regs::TevStageConfig::Source;
                 using ColorModifier = Regs::TevStageConfig::ColorModifier;
                 using AlphaModifier = Regs::TevStageConfig::AlphaModifier;
                 using Operation = Regs::TevStageConfig::Operation;
 
-                auto GetColorSource = [&](Source source) -> Math::Vec3<u8> {
+                auto GetColorSource = [&](Source source) -> Math::Vec4<u8> {
                     switch (source) {
                     case Source::PrimaryColor:
-                        return primary_color.rgb();
+                        return primary_color;
 
                     case Source::Texture0:
-                        return texture_color.rgb();
+                        return texture_color[0];
+
+                    case Source::Texture1:
+                        return texture_color[1];
+
+                    case Source::Texture2:
+                        return texture_color[2];
 
                     case Source::Constant:
-                        return {tev_stage.const_r, tev_stage.const_g, tev_stage.const_b};
+                        return {tev_stage.const_r, tev_stage.const_g, tev_stage.const_b, tev_stage.const_a};
 
                     case Source::Previous:
-                        return combiner_output.rgb();
+                        return combiner_output;
 
                     default:
                         LOG_ERROR(HW_GPU, "Unknown color combiner source %d\n", (int)source);
+                        _dbg_assert_(HW_GPU, 0);
                         return {};
                     }
                 };
@@ -263,7 +258,13 @@ void ProcessTriangle(const VertexShader::OutputVertex& v0,
                         return primary_color.a();
 
                     case Source::Texture0:
-                        return texture_color.a();
+                        return texture_color[0].a();
+
+                    case Source::Texture1:
+                        return texture_color[1].a();
+
+                    case Source::Texture2:
+                        return texture_color[2].a();
 
                     case Source::Constant:
                         return tev_stage.const_a;
@@ -273,17 +274,23 @@ void ProcessTriangle(const VertexShader::OutputVertex& v0,
 
                     default:
                         LOG_ERROR(HW_GPU, "Unknown alpha combiner source %d\n", (int)source);
+                        _dbg_assert_(HW_GPU, 0);
                         return 0;
                     }
                 };
 
-                auto GetColorModifier = [](ColorModifier factor, const Math::Vec3<u8>& values) -> Math::Vec3<u8> {
+                auto GetColorModifier = [](ColorModifier factor, const Math::Vec4<u8>& values) -> Math::Vec3<u8> {
                     switch (factor)
                     {
                     case ColorModifier::SourceColor:
-                        return values;
+                        return values.rgb();
+
+                    case ColorModifier::SourceAlpha:
+                        return { values.a(), values.a(), values.a() };
+
                     default:
                         LOG_ERROR(HW_GPU, "Unknown color factor %d\n", (int)factor);
+                        _dbg_assert_(HW_GPU, 0);
                         return {};
                     }
                 };
@@ -292,8 +299,13 @@ void ProcessTriangle(const VertexShader::OutputVertex& v0,
                     switch (factor) {
                     case AlphaModifier::SourceAlpha:
                         return value;
+
+                    case AlphaModifier::OneMinusSourceAlpha:
+                        return 255 - value;
+
                     default:
-                        LOG_ERROR(HW_GPU, "Unknown color factor %d\n", (int)factor);
+                        LOG_ERROR(HW_GPU, "Unknown alpha factor %d\n", (int)factor);
+                        _dbg_assert_(HW_GPU, 0);
                         return 0;
                     }
                 };
@@ -306,8 +318,21 @@ void ProcessTriangle(const VertexShader::OutputVertex& v0,
                     case Operation::Modulate:
                         return ((input[0] * input[1]) / 255).Cast<u8>();
 
+                    case Operation::Add:
+                    {
+                        auto result = input[0] + input[1];
+                        result.r() = std::min(255, result.r());
+                        result.g() = std::min(255, result.g());
+                        result.b() = std::min(255, result.b());
+                        return result.Cast<u8>();
+                    }
+
+                    case Operation::Lerp:
+                        return ((input[0] * input[2] + input[1] * (Math::MakeVec<u8>(255, 255, 255) - input[2]).Cast<u8>()) / 255).Cast<u8>();
+
                     default:
                         LOG_ERROR(HW_GPU, "Unknown color combiner operation %d\n", (int)op);
+                        _dbg_assert_(HW_GPU, 0);
                         return {};
                     }
                 };
@@ -320,8 +345,15 @@ void ProcessTriangle(const VertexShader::OutputVertex& v0,
                     case Operation::Modulate:
                         return input[0] * input[1] / 255;
 
+                    case Operation::Add:
+                        return std::min(255, input[0] + input[1]);
+
+                    case Operation::Lerp:
+                        return (input[0] * input[2] + input[1] * (255 - input[2])) / 255;
+
                     default:
                         LOG_ERROR(HW_GPU, "Unknown alpha combiner operation %d\n", (int)op);
+                        _dbg_assert_(HW_GPU, 0);
                         return 0;
                     }
                 };
diff --git a/src/video_core/rasterizer.h b/src/video_core/rasterizer.h
index 500be9462..42148f8b1 100644
--- a/src/video_core/rasterizer.h
+++ b/src/video_core/rasterizer.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/video_core/renderer_base.h b/src/video_core/renderer_base.h
index bce402b88..b77f29c11 100644
--- a/src/video_core/renderer_base.h
+++ b/src/video_core/renderer_base.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/video_core/renderer_opengl/gl_shader_util.cpp b/src/video_core/renderer_opengl/gl_shader_util.cpp
index d0f82e6cd..e982e3746 100644
--- a/src/video_core/renderer_opengl/gl_shader_util.cpp
+++ b/src/video_core/renderer_opengl/gl_shader_util.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "gl_shader_util.h"
diff --git a/src/video_core/renderer_opengl/gl_shader_util.h b/src/video_core/renderer_opengl/gl_shader_util.h
index 986cbabc0..9b93a8a0c 100644
--- a/src/video_core/renderer_opengl/gl_shader_util.h
+++ b/src/video_core/renderer_opengl/gl_shader_util.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/video_core/renderer_opengl/gl_shaders.h b/src/video_core/renderer_opengl/gl_shaders.h
index 0f88ab802..746a37afe 100644
--- a/src/video_core/renderer_opengl/gl_shaders.h
+++ b/src/video_core/renderer_opengl/gl_shaders.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/video_core/renderer_opengl/renderer_opengl.cpp b/src/video_core/renderer_opengl/renderer_opengl.cpp
index e2caeeb8f..4df3a5e25 100644
--- a/src/video_core/renderer_opengl/renderer_opengl.cpp
+++ b/src/video_core/renderer_opengl/renderer_opengl.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "core/hw/gpu.h"
@@ -240,14 +240,14 @@ MathUtil::Rectangle<unsigned> RendererOpenGL::GetViewportExtent() {
     MathUtil::Rectangle<unsigned> viewport_extent;
     if (window_aspect_ratio > emulation_aspect_ratio) {
         // Window is narrower than the emulation content => apply borders to the top and bottom
-        unsigned viewport_height = std::round(emulation_aspect_ratio * framebuffer_width);
+        unsigned viewport_height = static_cast<unsigned>(std::round(emulation_aspect_ratio * framebuffer_width));
         viewport_extent.left = 0;
         viewport_extent.top = (framebuffer_height - viewport_height) / 2;
         viewport_extent.right = viewport_extent.left + framebuffer_width;
         viewport_extent.bottom = viewport_extent.top + viewport_height;
     } else {
         // Otherwise, apply borders to the left and right sides of the window.
-        unsigned viewport_width = std::round(framebuffer_height / emulation_aspect_ratio);
+        unsigned viewport_width = static_cast<unsigned>(std::round(framebuffer_height / emulation_aspect_ratio));
         viewport_extent.left = (framebuffer_width - viewport_width) / 2;
         viewport_extent.top = 0;
         viewport_extent.right = viewport_extent.left + viewport_width;
diff --git a/src/video_core/renderer_opengl/renderer_opengl.h b/src/video_core/renderer_opengl/renderer_opengl.h
index 7fdcec731..cf78c1e77 100644
--- a/src/video_core/renderer_opengl/renderer_opengl.h
+++ b/src/video_core/renderer_opengl/renderer_opengl.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/video_core/utils.cpp b/src/video_core/utils.cpp
index f1156a493..c7cc93cea 100644
--- a/src/video_core/utils.cpp
+++ b/src/video_core/utils.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include <stdio.h>
diff --git a/src/video_core/utils.h b/src/video_core/utils.h
index 21380a908..63ebccbde 100644
--- a/src/video_core/utils.h
+++ b/src/video_core/utils.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
diff --git a/src/video_core/vertex_shader.cpp b/src/video_core/vertex_shader.cpp
index 477e78cfe..bed5081a0 100644
--- a/src/video_core/vertex_shader.cpp
+++ b/src/video_core/vertex_shader.cpp
@@ -1,17 +1,26 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
+#include <stack>
+
 #include <boost/range/algorithm.hpp>
 
 #include <common/file_util.h>
 
 #include <core/mem_map.h>
 
-#include "debug_utils/debug_utils.h"
+#include <nihstro/shader_bytecode.h>
+
 
 #include "pica.h"
 #include "vertex_shader.h"
+#include "debug_utils/debug_utils.h"
+
+using nihstro::Instruction;
+using nihstro::RegisterType;
+using nihstro::SourceRegister;
+using nihstro::SwizzlePattern;
 
 namespace Pica {
 
@@ -19,13 +28,14 @@ namespace VertexShader {
 
 static struct {
     Math::Vec4<float24> f[96];
-} shader_uniforms;
 
+    std::array<bool,16> b;
+} shader_uniforms;
 
 // TODO: Not sure where the shader binary and swizzle patterns are supposed to be loaded to!
 // For now, we just keep these local arrays around.
-static u32 shader_memory[1024];
-static u32 swizzle_data[1024];
+static std::array<u32, 1024> shader_memory;
+static std::array<u32, 1024> swizzle_data;
 
 void SubmitShaderMemoryChange(u32 addr, u32 value)
 {
@@ -42,6 +52,21 @@ Math::Vec4<float24>& GetFloatUniform(u32 index)
     return shader_uniforms.f[index];
 }
 
+bool& GetBoolUniform(u32 index)
+{
+    return shader_uniforms.b[index];
+}
+
+const std::array<u32, 1024>& GetShaderBinary()
+{
+    return shader_memory;
+}
+
+const std::array<u32, 1024>& GetSwizzlePatterns()
+{
+    return swizzle_data;
+}
+
 struct VertexShaderState {
     u32* program_counter;
 
@@ -49,13 +74,23 @@ struct VertexShaderState {
     float24* output_register_table[7*4];
 
     Math::Vec4<float24> temporary_registers[16];
-    bool status_registers[2];
+    bool conditional_code[2];
+
+    // Two Address registers and one loop counter
+    // TODO: How many bits do these actually have?
+    s32 address_registers[3];
 
     enum {
         INVALID_ADDRESS = 0xFFFFFFFF
     };
-    u32 call_stack[8]; // TODO: What is the maximal call stack depth?
-    u32* call_stack_pointer;
+
+    struct CallStackElement {
+        u32 final_address;
+        u32 return_address;
+    };
+
+    // TODO: Is there a maximal size for this?
+    std::stack<CallStackElement> call_stack;
 
     struct {
         u32 max_offset; // maximum program counter ever reached
@@ -64,49 +99,105 @@ struct VertexShaderState {
 };
 
 static void ProcessShaderCode(VertexShaderState& state) {
+
+    // Placeholder for invalid inputs
+    static float24 dummy_vec4_float24[4];
+
     while (true) {
-        bool increment_pc = true;
+        if (!state.call_stack.empty()) {
+            if (state.program_counter - shader_memory.data() == state.call_stack.top().final_address) {
+                state.program_counter = &shader_memory[state.call_stack.top().return_address];
+                state.call_stack.pop();
+
+                // TODO: Is "trying again" accurate to hardware?
+                continue;
+            }
+        }
+
         bool exit_loop = false;
         const Instruction& instr = *(const Instruction*)state.program_counter;
-        state.debug.max_offset = std::max<u32>(state.debug.max_offset, 1 + (state.program_counter - shader_memory));
-
-        const float24* src1_ = (instr.common.src1 < 0x10) ? state.input_register_table[instr.common.src1.GetIndex()]
-                             : (instr.common.src1 < 0x20) ? &state.temporary_registers[instr.common.src1.GetIndex()].x
-                             : (instr.common.src1 < 0x80) ? &shader_uniforms.f[instr.common.src1.GetIndex()].x
-                             : nullptr;
-        const float24* src2_ = (instr.common.src2 < 0x10) ? state.input_register_table[instr.common.src2.GetIndex()]
-                             : &state.temporary_registers[instr.common.src2.GetIndex()].x;
-        float24* dest = (instr.common.dest < 0x08) ? state.output_register_table[4*instr.common.dest.GetIndex()]
-                      : (instr.common.dest < 0x10) ? nullptr
-                      : (instr.common.dest < 0x20) ? &state.temporary_registers[instr.common.dest.GetIndex()][0]
-                      : nullptr;
-
         const SwizzlePattern& swizzle = *(SwizzlePattern*)&swizzle_data[instr.common.operand_desc_id];
-        const bool negate_src1 = (swizzle.negate != 0);
 
-        float24 src1[4] = {
-            src1_[(int)swizzle.GetSelectorSrc1(0)],
-            src1_[(int)swizzle.GetSelectorSrc1(1)],
-            src1_[(int)swizzle.GetSelectorSrc1(2)],
-            src1_[(int)swizzle.GetSelectorSrc1(3)],
+        auto call = [&](VertexShaderState& state, u32 offset, u32 num_instructions, u32 return_offset) {
+            state.program_counter = &shader_memory[offset] - 1; // -1 to make sure when incrementing the PC we end up at the correct offset
+            state.call_stack.push({ offset + num_instructions, return_offset });
         };
-        if (negate_src1) {
-            src1[0] = src1[0] * float24::FromFloat32(-1);
-            src1[1] = src1[1] * float24::FromFloat32(-1);
-            src1[2] = src1[2] * float24::FromFloat32(-1);
-            src1[3] = src1[3] * float24::FromFloat32(-1);
-        }
-        const float24 src2[4] = {
-            src2_[(int)swizzle.GetSelectorSrc2(0)],
-            src2_[(int)swizzle.GetSelectorSrc2(1)],
-            src2_[(int)swizzle.GetSelectorSrc2(2)],
-            src2_[(int)swizzle.GetSelectorSrc2(3)],
+        u32 binary_offset = state.program_counter - shader_memory.data();
+
+        state.debug.max_offset = std::max<u32>(state.debug.max_offset, 1 + binary_offset);
+
+        auto LookupSourceRegister = [&](const SourceRegister& source_reg) -> const float24* {
+            switch (source_reg.GetRegisterType()) {
+            case RegisterType::Input:
+                return state.input_register_table[source_reg.GetIndex()];
+
+            case RegisterType::Temporary:
+                return &state.temporary_registers[source_reg.GetIndex()].x;
+
+            case RegisterType::FloatUniform:
+                return &shader_uniforms.f[source_reg.GetIndex()].x;
+
+            default:
+                return dummy_vec4_float24;
+            }
         };
 
-        switch (instr.opcode) {
+        switch (instr.opcode.GetInfo().type) {
+        case Instruction::OpCodeType::Arithmetic:
+        {
+            bool is_inverted = 0 != (instr.opcode.GetInfo().subtype & Instruction::OpCodeInfo::SrcInversed);
+            if (is_inverted) {
+                // TODO: We don't really support this properly: For instance, the address register
+                //       offset needs to be applied to SRC2 instead, etc.
+                //       For now, we just abort in this situation.
+                LOG_CRITICAL(HW_GPU, "Bad condition...");
+                exit(0);
+            }
+
+            const int address_offset = (instr.common.address_register_index == 0)
+                                       ? 0 : state.address_registers[instr.common.address_register_index - 1];
+
+            const float24* src1_ = LookupSourceRegister(instr.common.GetSrc1(is_inverted) + address_offset);
+            const float24* src2_ = LookupSourceRegister(instr.common.GetSrc2(is_inverted));
+
+            const bool negate_src1 = ((bool)swizzle.negate_src1 != false);
+            const bool negate_src2 = ((bool)swizzle.negate_src2 != false);
+
+            float24 src1[4] = {
+                src1_[(int)swizzle.GetSelectorSrc1(0)],
+                src1_[(int)swizzle.GetSelectorSrc1(1)],
+                src1_[(int)swizzle.GetSelectorSrc1(2)],
+                src1_[(int)swizzle.GetSelectorSrc1(3)],
+            };
+            if (negate_src1) {
+                src1[0] = src1[0] * float24::FromFloat32(-1);
+                src1[1] = src1[1] * float24::FromFloat32(-1);
+                src1[2] = src1[2] * float24::FromFloat32(-1);
+                src1[3] = src1[3] * float24::FromFloat32(-1);
+            }
+            float24 src2[4] = {
+                src2_[(int)swizzle.GetSelectorSrc2(0)],
+                src2_[(int)swizzle.GetSelectorSrc2(1)],
+                src2_[(int)swizzle.GetSelectorSrc2(2)],
+                src2_[(int)swizzle.GetSelectorSrc2(3)],
+            };
+            if (negate_src2) {
+                src2[0] = src2[0] * float24::FromFloat32(-1);
+                src2[1] = src2[1] * float24::FromFloat32(-1);
+                src2[2] = src2[2] * float24::FromFloat32(-1);
+                src2[3] = src2[3] * float24::FromFloat32(-1);
+            }
+
+            float24* dest = (instr.common.dest < 0x08) ? state.output_register_table[4*instr.common.dest.GetIndex()]
+                        : (instr.common.dest < 0x10) ? dummy_vec4_float24
+                        : (instr.common.dest < 0x20) ? &state.temporary_registers[instr.common.dest.GetIndex()][0]
+                        : dummy_vec4_float24;
+
+            state.debug.max_opdesc_id = std::max<u32>(state.debug.max_opdesc_id, 1+instr.common.operand_desc_id);
+
+            switch (instr.opcode.EffectiveOpCode()) {
             case Instruction::OpCode::ADD:
             {
-                state.debug.max_opdesc_id = std::max<u32>(state.debug.max_opdesc_id, 1+instr.common.operand_desc_id);
                 for (int i = 0; i < 4; ++i) {
                     if (!swizzle.DestComponentEnabled(i))
                         continue;
@@ -119,7 +210,6 @@ static void ProcessShaderCode(VertexShaderState& state) {
 
             case Instruction::OpCode::MUL:
             {
-                state.debug.max_opdesc_id = std::max<u32>(state.debug.max_opdesc_id, 1+instr.common.operand_desc_id);
                 for (int i = 0; i < 4; ++i) {
                     if (!swizzle.DestComponentEnabled(i))
                         continue;
@@ -130,10 +220,18 @@ static void ProcessShaderCode(VertexShaderState& state) {
                 break;
             }
 
+            case Instruction::OpCode::MAX:
+                for (int i = 0; i < 4; ++i) {
+                    if (!swizzle.DestComponentEnabled(i))
+                        continue;
+
+                    dest[i] = std::max(src1[i], src2[i]);
+                }
+                break;
+
             case Instruction::OpCode::DP3:
             case Instruction::OpCode::DP4:
             {
-                state.debug.max_opdesc_id = std::max<u32>(state.debug.max_opdesc_id, 1+instr.common.operand_desc_id);
                 float24 dot = float24::FromFloat32(0.f);
                 int num_components = (instr.opcode == Instruction::OpCode::DP3) ? 3 : 4;
                 for (int i = 0; i < num_components; ++i)
@@ -151,7 +249,6 @@ static void ProcessShaderCode(VertexShaderState& state) {
             // Reciprocal
             case Instruction::OpCode::RCP:
             {
-                state.debug.max_opdesc_id = std::max<u32>(state.debug.max_opdesc_id, 1+instr.common.operand_desc_id);
                 for (int i = 0; i < 4; ++i) {
                     if (!swizzle.DestComponentEnabled(i))
                         continue;
@@ -167,7 +264,6 @@ static void ProcessShaderCode(VertexShaderState& state) {
             // Reciprocal Square Root
             case Instruction::OpCode::RSQ:
             {
-                state.debug.max_opdesc_id = std::max<u32>(state.debug.max_opdesc_id, 1+instr.common.operand_desc_id);
                 for (int i = 0; i < 4; ++i) {
                     if (!swizzle.DestComponentEnabled(i))
                         continue;
@@ -180,9 +276,21 @@ static void ProcessShaderCode(VertexShaderState& state) {
                 break;
             }
 
+            case Instruction::OpCode::MOVA:
+            {
+                for (int i = 0; i < 2; ++i) {
+                    if (!swizzle.DestComponentEnabled(i))
+                        continue;
+
+                    // TODO: Figure out how the rounding is done on hardware
+                    state.address_registers[i] = static_cast<s32>(src1[i].ToFloat32());
+                }
+
+                break;
+            }
+
             case Instruction::OpCode::MOV:
             {
-                state.debug.max_opdesc_id = std::max<u32>(state.debug.max_opdesc_id, 1+instr.common.operand_desc_id);
                 for (int i = 0; i < 4; ++i) {
                     if (!swizzle.DestComponentEnabled(i))
                         continue;
@@ -192,39 +300,137 @@ static void ProcessShaderCode(VertexShaderState& state) {
                 break;
             }
 
-            case Instruction::OpCode::RET:
-                if (*state.call_stack_pointer == VertexShaderState::INVALID_ADDRESS) {
-                    exit_loop = true;
-                } else {
-                    // Jump back to call stack position, invalidate call stack entry, move up call stack pointer
-                    state.program_counter = &shader_memory[*state.call_stack_pointer];
-                    *state.call_stack_pointer-- = VertexShaderState::INVALID_ADDRESS;
+            case Instruction::OpCode::CMP:
+                for (int i = 0; i < 2; ++i) {
+                    // TODO: Can you restrict to one compare via dest masking?
+
+                    auto compare_op = instr.common.compare_op;
+                    auto op = (i == 0) ? compare_op.x.Value() : compare_op.y.Value();
+
+                    switch (op) {
+                        case compare_op.Equal:
+                            state.conditional_code[i] = (src1[i] == src2[i]);
+                            break;
+
+                        case compare_op.NotEqual:
+                            state.conditional_code[i] = (src1[i] != src2[i]);
+                            break;
+
+                        case compare_op.LessThan:
+                            state.conditional_code[i] = (src1[i] <  src2[i]);
+                            break;
+
+                        case compare_op.LessEqual:
+                            state.conditional_code[i] = (src1[i] <= src2[i]);
+                            break;
+
+                        case compare_op.GreaterThan:
+                            state.conditional_code[i] = (src1[i] >  src2[i]);
+                            break;
+
+                        case compare_op.GreaterEqual:
+                            state.conditional_code[i] = (src1[i] >= src2[i]);
+                            break;
+
+                        default:
+                            LOG_ERROR(HW_GPU, "Unknown compare mode %x", static_cast<int>(op));
+                            break;
+                    }
                 }
+                break;
 
+            default:
+                LOG_ERROR(HW_GPU, "Unhandled arithmetic instruction: 0x%02x (%s): 0x%08x",
+                          (int)instr.opcode.Value(), instr.opcode.GetInfo().name, instr.hex);
+                _dbg_assert_(HW_GPU, 0);
+                break;
+            }
+
+            break;
+        }
+        default:
+            // Handle each instruction on its own
+            switch (instr.opcode) {
+            case Instruction::OpCode::END:
+                exit_loop = true;
                 break;
 
             case Instruction::OpCode::CALL:
-                increment_pc = false;
+                call(state,
+                     instr.flow_control.dest_offset,
+                     instr.flow_control.num_instructions,
+                     binary_offset + 1);
+                break;
 
-                _dbg_assert_(HW_GPU, state.call_stack_pointer - state.call_stack < sizeof(state.call_stack));
+            case Instruction::OpCode::NOP:
+                break;
+
+            case Instruction::OpCode::IFU:
+                if (shader_uniforms.b[instr.flow_control.bool_uniform_id]) {
+                    call(state,
+                         binary_offset + 1,
+                         instr.flow_control.dest_offset - binary_offset - 1,
+                         instr.flow_control.dest_offset + instr.flow_control.num_instructions);
+                } else {
+                    call(state,
+                         instr.flow_control.dest_offset,
+                         instr.flow_control.num_instructions,
+                         instr.flow_control.dest_offset + instr.flow_control.num_instructions);
+                }
 
-                *++state.call_stack_pointer = state.program_counter - shader_memory;
-                // TODO: Does this offset refer to the beginning of shader memory?
-                state.program_counter = &shader_memory[instr.flow_control.offset_words];
                 break;
 
-            case Instruction::OpCode::FLS:
-                // TODO: Do whatever needs to be done here?
+            case Instruction::OpCode::IFC:
+            {
+                // TODO: Do we need to consider swizzlers here?
+
+                auto flow_control = instr.flow_control;
+                bool results[3] = { (bool)flow_control.refx == state.conditional_code[0],
+                                    (bool)flow_control.refy == state.conditional_code[1] };
+
+                switch (flow_control.op) {
+                case flow_control.Or:
+                    results[2] = results[0] || results[1];
+                    break;
+
+                case flow_control.And:
+                    results[2] = results[0] && results[1];
+                    break;
+
+                case flow_control.JustX:
+                    results[2] = results[0];
+                    break;
+
+                case flow_control.JustY:
+                    results[2] = results[1];
+                    break;
+                }
+
+                if (results[2]) {
+                    call(state,
+                         binary_offset + 1,
+                         instr.flow_control.dest_offset - binary_offset - 1,
+                         instr.flow_control.dest_offset + instr.flow_control.num_instructions);
+                } else {
+                    call(state,
+                         instr.flow_control.dest_offset,
+                         instr.flow_control.num_instructions,
+                         instr.flow_control.dest_offset + instr.flow_control.num_instructions);
+                }
+
                 break;
+            }
 
             default:
                 LOG_ERROR(HW_GPU, "Unhandled instruction: 0x%02x (%s): 0x%08x",
-                          (int)instr.opcode.Value(), instr.GetOpCodeName().c_str(), instr.hex);
+                          (int)instr.opcode.Value(), instr.opcode.GetInfo().name, instr.hex);
                 break;
+            }
+
+            break;
         }
 
-        if (increment_pc)
-            ++state.program_counter;
+        ++state.program_counter;
 
         if (exit_loop)
             break;
@@ -263,6 +469,10 @@ OutputVertex RunShader(const InputVertex& input, int num_attributes)
 
     // Setup output register table
     OutputVertex ret;
+    // Zero output so that attributes which aren't output won't have denormals in them, which will
+    // slow us down later.
+    memset(&ret, 0, sizeof(ret));
+
     for (int i = 0; i < 7; ++i) {
         const auto& output_register_map = registers.vs_output_attributes[i];
 
@@ -275,13 +485,11 @@ OutputVertex RunShader(const InputVertex& input, int num_attributes)
             state.output_register_table[4*i+comp] = ((float24*)&ret) + semantics[comp];
     }
 
-    state.status_registers[0] = false;
-    state.status_registers[1] = false;
-    boost::fill(state.call_stack, VertexShaderState::INVALID_ADDRESS);
-    state.call_stack_pointer = &state.call_stack[0];
+    state.conditional_code[0] = false;
+    state.conditional_code[1] = false;
 
     ProcessShaderCode(state);
-    DebugUtils::DumpShader(shader_memory, state.debug.max_offset, swizzle_data,
+    DebugUtils::DumpShader(shader_memory.data(), state.debug.max_offset, swizzle_data.data(),
                            state.debug.max_opdesc_id, registers.vs_main_offset,
                            registers.vs_output_attributes);
 
diff --git a/src/video_core/vertex_shader.h b/src/video_core/vertex_shader.h
index bfb6fb6e3..af3fb2a2f 100644
--- a/src/video_core/vertex_shader.h
+++ b/src/video_core/vertex_shader.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once
@@ -27,15 +27,18 @@ struct OutputVertex {
     Math::Vec4<float24> dummy; // quaternions (not implemented, yet)
     Math::Vec4<float24> color;
     Math::Vec2<float24> tc0;
+    Math::Vec2<float24> tc1;
+    float24 pad[6];
+    Math::Vec2<float24> tc2;
 
     // Padding for optimal alignment
-    float24 pad[14];
+    float24 pad2[4];
 
     // Attributes used to store intermediate results
 
     // position after perspective divide
     Math::Vec3<float24> screenpos;
-    float24 pad2;
+    float24 pad3;
 
     // Linear interpolation
     // factor: 0=this, 1=vtx
@@ -44,6 +47,8 @@ struct OutputVertex {
 
         // TODO: Should perform perspective correct interpolation here...
         tc0 = tc0 * factor + vtx.tc0 * (float24::FromFloat32(1) - factor);
+        tc1 = tc1 * factor + vtx.tc1 * (float24::FromFloat32(1) - factor);
+        tc2 = tc2 * factor + vtx.tc2 * (float24::FromFloat32(1) - factor);
 
         screenpos = screenpos * factor + vtx.screenpos * (float24::FromFloat32(1) - factor);
 
@@ -61,222 +66,16 @@ struct OutputVertex {
 static_assert(std::is_pod<OutputVertex>::value, "Structure is not POD");
 static_assert(sizeof(OutputVertex) == 32 * sizeof(float), "OutputVertex has invalid size");
 
-union Instruction {
-    enum class OpCode : u32 {
-        ADD = 0x0,
-        DP3 = 0x1,
-        DP4 = 0x2,
-
-        MUL = 0x8,
-
-        MAX = 0xC,
-        MIN = 0xD,
-        RCP = 0xE,
-        RSQ = 0xF,
-
-        MOV = 0x13,
-
-        RET = 0x21,
-        FLS = 0x22, // Flush
-        CALL = 0x24,
-    };
-
-    std::string GetOpCodeName() const {
-        std::map<OpCode, std::string> map = {
-            { OpCode::ADD, "ADD" },
-            { OpCode::DP3, "DP3" },
-            { OpCode::DP4, "DP4" },
-            { OpCode::MUL, "MUL" },
-            { OpCode::MAX, "MAX" },
-            { OpCode::MIN, "MIN" },
-            { OpCode::RCP, "RCP" },
-            { OpCode::RSQ, "RSQ" },
-            { OpCode::MOV, "MOV" },
-            { OpCode::RET, "RET" },
-            { OpCode::FLS, "FLS" },
-        };
-        auto it = map.find(opcode);
-        if (it == map.end())
-            return "UNK";
-        else
-            return it->second;
-    }
-
-    u32 hex;
-
-    BitField<0x1a, 0x6, OpCode> opcode;
-
-    // General notes:
-    //
-    // When two input registers are used, one of them uses a 5-bit index while the other
-    // one uses a 7-bit index. This is because at most one floating point uniform may be used
-    // as an input.
-
-
-    // Format used e.g. by arithmetic instructions and comparisons
-    // "src1" and "src2" specify register indices (i.e. indices referring to groups of 4 floats),
-    // while "dest" addresses individual floats.
-    union {
-        BitField<0x00, 0x5, u32> operand_desc_id;
-
-        template<class BitFieldType>
-        struct SourceRegister : BitFieldType {
-            enum RegisterType {
-                Input,
-                Temporary,
-                FloatUniform
-            };
-
-            RegisterType GetRegisterType() const {
-                if (BitFieldType::Value() < 0x10)
-                    return Input;
-                else if (BitFieldType::Value() < 0x20)
-                    return Temporary;
-                else
-                    return FloatUniform;
-            }
-
-            int GetIndex() const {
-                if (GetRegisterType() == Input)
-                    return BitFieldType::Value();
-                else if (GetRegisterType() == Temporary)
-                    return BitFieldType::Value() - 0x10;
-                else // if (GetRegisterType() == FloatUniform)
-                    return BitFieldType::Value() - 0x20;
-            }
-
-            std::string GetRegisterName() const {
-                std::map<RegisterType, std::string> type = {
-                    { Input, "i" },
-                    { Temporary, "t" },
-                    { FloatUniform, "f" },
-                };
-                return type[GetRegisterType()] + std::to_string(GetIndex());
-            }
-        };
-
-        SourceRegister<BitField<0x07, 0x5, u32>> src2;
-        SourceRegister<BitField<0x0c, 0x7, u32>> src1;
-
-        struct : BitField<0x15, 0x5, u32>
-        {
-            enum RegisterType {
-                Output,
-                Temporary,
-                Unknown
-            };
-            RegisterType GetRegisterType() const {
-                if (Value() < 0x8)
-                    return Output;
-                else if (Value() < 0x10)
-                    return Unknown;
-                else
-                    return Temporary;
-            }
-            int GetIndex() const {
-                if (GetRegisterType() == Output)
-                    return Value();
-                else if (GetRegisterType() == Temporary)
-                    return Value() - 0x10;
-                else
-                    return Value();
-            }
-            std::string GetRegisterName() const {
-                std::map<RegisterType, std::string> type = {
-                    { Output, "o" },
-                    { Temporary, "t" },
-                    { Unknown, "u" }
-                };
-                return type[GetRegisterType()] + std::to_string(GetIndex());
-            }
-        } dest;
-    } common;
-
-    // Format used for flow control instructions ("if")
-    union {
-        BitField<0x00, 0x8, u32> num_instructions;
-        BitField<0x0a, 0xc, u32> offset_words;
-    } flow_control;
-};
-static_assert(std::is_standard_layout<Instruction>::value, "Structure is not using standard layout!");
-
-union SwizzlePattern {
-    u32 hex;
-
-    enum class Selector : u32 {
-        x = 0,
-        y = 1,
-        z = 2,
-        w = 3
-    };
-
-    Selector GetSelectorSrc1(int comp) const {
-        Selector selectors[] = {
-            src1_selector_0, src1_selector_1, src1_selector_2, src1_selector_3
-        };
-        return selectors[comp];
-    }
-
-    Selector GetSelectorSrc2(int comp) const {
-        Selector selectors[] = {
-            src2_selector_0, src2_selector_1, src2_selector_2, src2_selector_3
-        };
-        return selectors[comp];
-    }
-
-    bool DestComponentEnabled(int i) const {
-        return (dest_mask & (0x8 >> i)) != 0;
-    }
-
-    std::string SelectorToString(bool src2) const {
-        std::map<Selector, std::string> map = {
-            { Selector::x, "x" },
-            { Selector::y, "y" },
-            { Selector::z, "z" },
-            { Selector::w, "w" }
-        };
-        std::string ret;
-        for (int i = 0; i < 4; ++i) {
-            ret += map.at(src2 ? GetSelectorSrc2(i) : GetSelectorSrc1(i));
-        }
-        return ret;
-    }
-
-    std::string DestMaskToString() const {
-        std::string ret;
-        for (int i = 0; i < 4; ++i) {
-            if (!DestComponentEnabled(i))
-                ret += "_";
-            else
-                ret += "xyzw"[i];
-        }
-        return ret;
-    }
-
-    // Components of "dest" that should be written to: LSB=dest.w, MSB=dest.x
-    BitField< 0, 4, u32> dest_mask;
-
-    BitField< 4, 1, u32> negate; // negates src1
-
-    BitField< 5, 2, Selector> src1_selector_3;
-    BitField< 7, 2, Selector> src1_selector_2;
-    BitField< 9, 2, Selector> src1_selector_1;
-    BitField<11, 2, Selector> src1_selector_0;
-
-    BitField<14, 2, Selector> src2_selector_3;
-    BitField<16, 2, Selector> src2_selector_2;
-    BitField<18, 2, Selector> src2_selector_1;
-    BitField<20, 2, Selector> src2_selector_0;
-
-    BitField<31, 1, u32> flag; // not sure what this means, maybe it's the sign?
-};
-
 void SubmitShaderMemoryChange(u32 addr, u32 value);
 void SubmitSwizzleDataChange(u32 addr, u32 value);
 
 OutputVertex RunShader(const InputVertex& input, int num_attributes);
 
 Math::Vec4<float24>& GetFloatUniform(u32 index);
+bool& GetBoolUniform(u32 index);
+
+const std::array<u32, 1024>& GetShaderBinary();
+const std::array<u32, 1024>& GetSwizzlePatterns();
 
 } // namespace
 
diff --git a/src/video_core/video_core.cpp b/src/video_core/video_core.cpp
index 6791e4007..c9707e5f1 100644
--- a/src/video_core/video_core.cpp
+++ b/src/video_core/video_core.cpp
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #include "common/common.h"
diff --git a/src/video_core/video_core.h b/src/video_core/video_core.h
index 609aac513..b782f17bd 100644
--- a/src/video_core/video_core.h
+++ b/src/video_core/video_core.h
@@ -1,5 +1,5 @@
 // Copyright 2014 Citra Emulator Project
-// Licensed under GPLv2
+// Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
 #pragma once