diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3070844..994aff2 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,8 +1,9 @@
 cmake_minimum_required(VERSION 3.12)
 list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake)
-###############################################################################
+
+# ##############################################################################
 #
-###############################################################################
+# ##############################################################################
 project(dbt-rise-tgc VERSION 1.0.0)
 
 include(GNUInstallDirs)
@@ -14,59 +15,69 @@ find_package(Boost COMPONENTS coroutine REQUIRED)
 
 add_subdirectory(softfloat)
 
-set(LIB_SOURCES 
+set(LIB_SOURCES
     src/iss/plugin/instruction_count.cpp
-	src/iss/arch/tgc5c.cpp
-	src/vm/interp/vm_tgc5c.cpp
-	src/vm/fp_functions.cpp
+    src/iss/arch/tgc5c.cpp
+    src/vm/interp/vm_tgc5c.cpp
+    src/vm/fp_functions.cpp
+    src/iss/semihosting/semihosting.cpp
 )
+
 if(WITH_TCC)
-	list(APPEND LIB_SOURCES
-	   src/vm/tcc/vm_tgc5c.cpp
+    list(APPEND LIB_SOURCES
+        src/vm/tcc/vm_tgc5c.cpp
     )
 endif()
+
 if(WITH_LLVM)
-	list(APPEND LIB_SOURCES
-		src/vm/llvm/vm_tgc5c.cpp
-		src/vm/llvm/fp_impl.cpp
-	)
-endif()
-if(WITH_ASMJIT)
-	list(APPEND LIB_SOURCES
-	   src/vm/asmjit/vm_tgc5c.cpp
+    list(APPEND LIB_SOURCES
+        src/vm/llvm/vm_tgc5c.cpp
+        src/vm/llvm/fp_impl.cpp
     )
 endif()
+
+if(WITH_ASMJIT)
+    list(APPEND LIB_SOURCES
+        src/vm/asmjit/vm_tgc5c.cpp
+    )
+endif()
+
 # library files
 FILE(GLOB GEN_ISS_SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src-gen/iss/arch/*.cpp)
 FILE(GLOB GEN_VM_SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src-gen/vm/interp/vm_*.cpp)
 FILE(GLOB GEN_YAML_SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/contrib/instr/*.yaml)
 list(APPEND LIB_SOURCES ${GEN_ISS_SOURCES} ${GEN_VM_SOURCES})
+
 foreach(FILEPATH ${GEN_ISS_SOURCES})
     get_filename_component(CORE ${FILEPATH} NAME_WE)
     string(TOUPPER ${CORE} CORE)
     list(APPEND LIB_DEFINES CORE_${CORE})
 endforeach()
+
 message(STATUS "Core defines are ${LIB_DEFINES}")
 
 if(WITH_LLVM)
-	FILE(GLOB LLVM_GEN_SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src-gen/vm/llvm/vm_*.cpp)
-	list(APPEND LIB_SOURCES ${LLVM_GEN_SOURCES})
+    FILE(GLOB LLVM_GEN_SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src-gen/vm/llvm/vm_*.cpp)
+    list(APPEND LIB_SOURCES ${LLVM_GEN_SOURCES})
 endif()
 
 if(WITH_TCC)
-	FILE(GLOB TCC_GEN_SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src-gen/vm/tcc/vm_*.cpp)
-	list(APPEND LIB_SOURCES ${TCC_GEN_SOURCES})
+    FILE(GLOB TCC_GEN_SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src-gen/vm/tcc/vm_*.cpp)
+    list(APPEND LIB_SOURCES ${TCC_GEN_SOURCES})
 endif()
+
 if(WITH_ASMJIT)
-	FILE(GLOB TCC_GEN_SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src-gen/vm/asmjit/vm_*.cpp)
-	list(APPEND LIB_SOURCES ${TCC_GEN_SOURCES})
+    FILE(GLOB TCC_GEN_SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src-gen/vm/asmjit/vm_*.cpp)
+    list(APPEND LIB_SOURCES ${TCC_GEN_SOURCES})
 endif()
+
 if(TARGET yaml-cpp::yaml-cpp)
-    list(APPEND LIB_SOURCES 
-    	src/iss/plugin/cycle_estimate.cpp
-	    src/iss/plugin/instruction_count.cpp
+    list(APPEND LIB_SOURCES
+        src/iss/plugin/cycle_estimate.cpp
+        src/iss/plugin/instruction_count.cpp
     )
 endif()
+
 # Define the library
 add_library(${PROJECT_NAME} SHARED ${LIB_SOURCES})
 
@@ -75,60 +86,67 @@ if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU")
 elseif("${CMAKE_CXX_COMPILER_ID}" STREQUAL "MSVC")
     target_compile_options(${PROJECT_NAME} PRIVATE /wd4293)
 endif()
+
 target_include_directories(${PROJECT_NAME} PUBLIC src)
 target_include_directories(${PROJECT_NAME} PUBLIC src-gen)
 
 target_force_link_libraries(${PROJECT_NAME} PRIVATE dbt-rise-core)
+
 # only re-export the include paths
 get_target_property(DBT_CORE_INCL dbt-rise-core INTERFACE_INCLUDE_DIRECTORIES)
 target_include_directories(${PROJECT_NAME} INTERFACE ${DBT_CORE_INCL})
 get_target_property(DBT_CORE_DEFS dbt-rise-core INTERFACE_COMPILE_DEFINITIONS)
-if(NOT (DBT_CORE_DEFS STREQUAL DBT_CORE_DEFS-NOTFOUND))
-	target_compile_definitions(${PROJECT_NAME} INTERFACE ${DBT_CORE_DEFS})
+
+if(NOT(DBT_CORE_DEFS STREQUAL DBT_CORE_DEFS-NOTFOUND))
+    target_compile_definitions(${PROJECT_NAME} INTERFACE ${DBT_CORE_DEFS})
 endif()
 
 target_link_libraries(${PROJECT_NAME} PUBLIC elfio::elfio softfloat scc-util Boost::coroutine)
+
 if(TARGET yaml-cpp::yaml-cpp)
-	target_compile_definitions(${PROJECT_NAME} PUBLIC WITH_PLUGINS)
-	target_link_libraries(${PROJECT_NAME} PUBLIC yaml-cpp::yaml-cpp)
+    target_compile_definitions(${PROJECT_NAME} PUBLIC WITH_PLUGINS)
+    target_link_libraries(${PROJECT_NAME} PUBLIC yaml-cpp::yaml-cpp)
 endif()
 
 if(WITH_LLVM)
     find_package(LLVM)
-	target_compile_definitions(${PROJECT_NAME} PUBLIC ${LLVM_DEFINITIONS})
-	target_include_directories(${PROJECT_NAME} PUBLIC ${LLVM_INCLUDE_DIRS})
-	if(BUILD_SHARED_LIBS)
-		target_link_libraries( ${PROJECT_NAME} PUBLIC ${LLVM_LIBRARIES})
-	endif()
+    target_compile_definitions(${PROJECT_NAME} PUBLIC ${LLVM_DEFINITIONS})
+    target_include_directories(${PROJECT_NAME} PUBLIC ${LLVM_INCLUDE_DIRS})
+
+    if(BUILD_SHARED_LIBS)
+        target_link_libraries(${PROJECT_NAME} PUBLIC ${LLVM_LIBRARIES})
+    endif()
 endif()
 
 set_target_properties(${PROJECT_NAME} PROPERTIES
-  VERSION ${PROJECT_VERSION}
-  FRAMEWORK FALSE
+    VERSION ${PROJECT_VERSION}
+    FRAMEWORK FALSE
 )
 install(TARGETS ${PROJECT_NAME} COMPONENT ${PROJECT_NAME}
-  EXPORT ${PROJECT_NAME}Targets            # for downstream dependencies
-  ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}  # static lib
-  RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}  # binaries
-  LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}  # shared lib
-  FRAMEWORK DESTINATION ${CMAKE_INSTALL_LIBDIR} # for mac
-  PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/${PROJECT_NAME} # headers for mac (note the different component -> different package)
-  INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}             # headers
+    EXPORT ${PROJECT_NAME}Targets # for downstream dependencies
+    ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} # static lib
+    RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} # binaries
+    LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} # shared lib
+    FRAMEWORK DESTINATION ${CMAKE_INSTALL_LIBDIR} # for mac
+    PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/${PROJECT_NAME} # headers for mac (note the different component -> different package)
+    INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} # headers
 )
 install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/incl/iss COMPONENT ${PROJECT_NAME}
-        DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} # target directory
-        FILES_MATCHING # install only matched files
-        PATTERN "*.h" # select header files
-        )
+    DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} # target directory
+    FILES_MATCHING # install only matched files
+    PATTERN "*.h" # select header files
+)
 install(FILES ${GEN_YAML_SOURCES} DESTINATION share/tgc-vp)
-###############################################################################
+
+# ##############################################################################
 #
-###############################################################################
+# ##############################################################################
 set(CMAKE_INSTALL_RPATH $ORIGIN/../${CMAKE_INSTALL_LIBDIR})
 project(tgc-sim)
 find_package(Boost COMPONENTS program_options thread REQUIRED)
 
 add_executable(${PROJECT_NAME} src/main.cpp)
+
 if(TARGET ${CORE_NAME}_cpp)
     list(APPEND TGC_SOURCES ${${CORE_NAME}_OUTPUT_FILES})
 else()
@@ -140,21 +158,20 @@ else()
 endif()
 
 foreach(F IN LISTS TGC_SOURCES)
-    if (${F} MATCHES ".*/arch/([^/]*)\.cpp")
-        string(REGEX REPLACE  ".*/([^/]*)\.cpp"  "\\1" CORE_NAME_LC ${F})
+    if(${F} MATCHES ".*/arch/([^/]*)\.cpp")
+        string(REGEX REPLACE ".*/([^/]*)\.cpp" "\\1" CORE_NAME_LC ${F})
         string(TOUPPER ${CORE_NAME_LC} CORE_NAME)
         target_compile_definitions(${PROJECT_NAME} PRIVATE CORE_${CORE_NAME})
     endif()
 endforeach()
 
-#if(WITH_LLVM)
-#    target_compile_definitions(${PROJECT_NAME} PRIVATE WITH_LLVM)
-#    #target_link_libraries(${PROJECT_NAME} PUBLIC ${llvm_libs})
-#endif()
-#if(WITH_TCC)
-#    target_compile_definitions(${PROJECT_NAME} PRIVATE WITH_TCC)
-#endif()
-
+# if(WITH_LLVM)
+# target_compile_definitions(${PROJECT_NAME} PRIVATE WITH_LLVM)
+# #target_link_libraries(${PROJECT_NAME} PUBLIC ${llvm_libs})
+# endif()
+# if(WITH_TCC)
+# target_compile_definitions(${PROJECT_NAME} PRIVATE WITH_TCC)
+# endif()
 target_link_libraries(${PROJECT_NAME} PUBLIC dbt-rise-tgc fmt::fmt)
 
 if(TARGET Boost::program_options)
@@ -162,78 +179,85 @@ if(TARGET Boost::program_options)
 else()
     target_link_libraries(${PROJECT_NAME} PUBLIC ${BOOST_program_options_LIBRARY})
 endif()
+
 target_link_libraries(${PROJECT_NAME} PUBLIC ${CMAKE_DL_LIBS})
-if (Tcmalloc_FOUND)
+
+if(Tcmalloc_FOUND)
     target_link_libraries(${PROJECT_NAME} PUBLIC ${Tcmalloc_LIBRARIES})
 endif(Tcmalloc_FOUND)
 
 install(TARGETS tgc-sim
-  EXPORT ${PROJECT_NAME}Targets            # for downstream dependencies
-  ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}  # static lib
-  RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}  # binaries
-  LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}  # shared lib
-  FRAMEWORK DESTINATION ${CMAKE_INSTALL_LIBDIR} # for mac
-  PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/${PROJECT_NAME}  # headers for mac (note the different component -> different package)
-  INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}             # headers
+    EXPORT ${PROJECT_NAME}Targets # for downstream dependencies
+    ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} # static lib
+    RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} # binaries
+    LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} # shared lib
+    FRAMEWORK DESTINATION ${CMAKE_INSTALL_LIBDIR} # for mac
+    PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/${PROJECT_NAME} # headers for mac (note the different component -> different package)
+    INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} # headers
 )
 
 if(BUILD_TESTING)
-  	# ... CMake code to create tests ...
-	add_test(NAME tgc-sim-interp
-	         COMMAND tgc-sim -f ${CMAKE_BINARY_DIR}/../../Firmwares/hello-world/hello --backend interp)
-	if(WITH_TCC)
-	add_test(NAME tgc-sim-tcc
-	         COMMAND tgc-sim -f ${CMAKE_BINARY_DIR}/../../Firmwares/hello-world/hello --backend tcc)
-	endif()
-	if(WITH_LLVM)
-	add_test(NAME tgc-sim-llvm
-	         COMMAND tgc-sim -f ${CMAKE_BINARY_DIR}/../../Firmwares/hello-world/hello --backend llvm)
-	endif()
+    # ... CMake code to create tests ...
+    add_test(NAME tgc-sim-interp
+        COMMAND tgc-sim -f ${CMAKE_BINARY_DIR}/../../Firmwares/hello-world/hello --backend interp)
+
+    if(WITH_TCC)
+        add_test(NAME tgc-sim-tcc
+            COMMAND tgc-sim -f ${CMAKE_BINARY_DIR}/../../Firmwares/hello-world/hello --backend tcc)
+    endif()
+
+    if(WITH_LLVM)
+        add_test(NAME tgc-sim-llvm
+            COMMAND tgc-sim -f ${CMAKE_BINARY_DIR}/../../Firmwares/hello-world/hello --backend llvm)
+    endif()
+
     if(WITH_ASMJIT)
-	add_test(NAME tgc-sim-asmjit
-	         COMMAND tgc-sim -f ${CMAKE_BINARY_DIR}/../../Firmwares/hello-world/hello --backend asmjit)
-	endif()
+        add_test(NAME tgc-sim-asmjit
+            COMMAND tgc-sim -f ${CMAKE_BINARY_DIR}/../../Firmwares/hello-world/hello --backend asmjit)
+    endif()
 endif()
-###############################################################################
+
+# ##############################################################################
 #
-###############################################################################
+# ##############################################################################
 if(TARGET scc-sysc)
-	project(dbt-rise-tgc_sc VERSION 1.0.0)
-	set(LIB_SOURCES 
-    	src/sysc/core_complex.cpp
-    	src/sysc/register_tgc_c.cpp
-	)
-	FILE(GLOB GEN_SC_SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src-gen/sysc/register_*.cpp)
-	list(APPEND LIB_SOURCES ${GEN_SC_SOURCES})
+    project(dbt-rise-tgc_sc VERSION 1.0.0)
+    set(LIB_SOURCES
+        src/sysc/core_complex.cpp
+        src/sysc/register_tgc_c.cpp
+    )
+    FILE(GLOB GEN_SC_SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src-gen/sysc/register_*.cpp)
+    list(APPEND LIB_SOURCES ${GEN_SC_SOURCES})
     add_library(${PROJECT_NAME} ${LIB_SOURCES})
     target_compile_definitions(${PROJECT_NAME} PUBLIC WITH_SYSTEMC)
     target_compile_definitions(${PROJECT_NAME} PRIVATE CORE_${CORE_NAME})
+
     foreach(F IN LISTS TGC_SOURCES)
-        if (${F} MATCHES ".*/arch/([^/]*)\.cpp")
-            string(REGEX REPLACE  ".*/([^/]*)\.cpp"  "\\1" CORE_NAME_LC ${F})
+        if(${F} MATCHES ".*/arch/([^/]*)\.cpp")
+            string(REGEX REPLACE ".*/([^/]*)\.cpp" "\\1" CORE_NAME_LC ${F})
             string(TOUPPER ${CORE_NAME_LC} CORE_NAME)
             target_compile_definitions(${PROJECT_NAME} PRIVATE CORE_${CORE_NAME})
         endif()
     endforeach()
+
     target_link_libraries(${PROJECT_NAME} PUBLIC dbt-rise-tgc scc-sysc)
-#    if(WITH_LLVM)
-#        target_link_libraries(${PROJECT_NAME} PUBLIC ${llvm_libs})
-#    endif()
-    
-	set(LIB_HEADERS ${CMAKE_CURRENT_SOURCE_DIR}/src/sysc/core_complex.h)
+
+    # if(WITH_LLVM)
+    # target_link_libraries(${PROJECT_NAME} PUBLIC ${llvm_libs})
+    # endif()
+    set(LIB_HEADERS ${CMAKE_CURRENT_SOURCE_DIR}/src/sysc/core_complex.h)
     set_target_properties(${PROJECT_NAME} PROPERTIES
-      VERSION ${PROJECT_VERSION}
-      FRAMEWORK FALSE
-      PUBLIC_HEADER "${LIB_HEADERS}" # specify the public headers
+        VERSION ${PROJECT_VERSION}
+        FRAMEWORK FALSE
+        PUBLIC_HEADER "${LIB_HEADERS}" # specify the public headers
     )
     install(TARGETS ${PROJECT_NAME} COMPONENT ${PROJECT_NAME}
-	  EXPORT ${PROJECT_NAME}Targets            # for downstream dependencies
-	  ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}  # static lib
-	  RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}  # binaries
-	  LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}  # shared lib
-	  FRAMEWORK DESTINATION ${CMAKE_INSTALL_LIBDIR} # for mac
-	  PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sysc   # headers for mac (note the different component -> different package)
-	  INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}             # headers
-	)    
+        EXPORT ${PROJECT_NAME}Targets # for downstream dependencies
+        ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} # static lib
+        RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} # binaries
+        LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} # shared lib
+        FRAMEWORK DESTINATION ${CMAKE_INSTALL_LIBDIR} # for mac
+        PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/sysc # headers for mac (note the different component -> different package)
+        INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} # headers
+    )
 endif()
-
diff --git a/gen_input/templates/asmjit/CORENAME.cpp.gtl b/gen_input/templates/asmjit/CORENAME.cpp.gtl
index 74f54d4..4dfc294 100644
--- a/gen_input/templates/asmjit/CORENAME.cpp.gtl
+++ b/gen_input/templates/asmjit/CORENAME.cpp.gtl
@@ -263,16 +263,24 @@ std::unique_ptr<vm_if> create<arch::${coreDef.name.toLowerCase()}>(arch::${coreD
 namespace iss {
 namespace {
 volatile std::array<bool, 2> dummy = {
-        core_factory::instance().register_creator("${coreDef.name.toLowerCase()}|m_p|asmjit", [](unsigned port, void*) -> std::tuple<cpu_ptr, vm_ptr>{
+        core_factory::instance().register_creator("${coreDef.name.toLowerCase()}|m_p|interp", [](unsigned port, void*) -> std::tuple<cpu_ptr, vm_ptr>{
             auto* cpu = new iss::arch::riscv_hart_m_p<iss::arch::${coreDef.name.toLowerCase()}>();
-            auto* vm = new asmjit::${coreDef.name.toLowerCase()}::vm_impl<arch::${coreDef.name.toLowerCase()}>(*cpu, false);
-            if (port != 0) debugger::server<debugger::gdb_session>::run_server(vm, port);
+		    auto vm = new interp::${coreDef.name.toLowerCase()}::vm_impl<arch::${coreDef.name.toLowerCase()}>(*cpu, false);
+		    if (port != 0) debugger::server<debugger::gdb_session>::run_server(vm, port);
+            if(init_data){
+                auto* cb = reinterpret_cast<std::function<void(arch_if*, arch::traits<arch::${coreDef.name.toLowerCase()}>::reg_t, arch::traits<arch::${coreDef.name.toLowerCase()}>::reg_t)>*>(init_data);
+                cpu->set_semihosting_callback(*cb);
+            }
             return {cpu_ptr{cpu}, vm_ptr{vm}};
         }),
-        core_factory::instance().register_creator("${coreDef.name.toLowerCase()}|mu_p|asmjit", [](unsigned port, void*) -> std::tuple<cpu_ptr, vm_ptr>{
+        core_factory::instance().register_creator("${coreDef.name.toLowerCase()}|mu_p|interp", [](unsigned port, void*) -> std::tuple<cpu_ptr, vm_ptr>{
             auto* cpu = new iss::arch::riscv_hart_mu_p<iss::arch::${coreDef.name.toLowerCase()}>();
-            auto* vm = new asmjit::${coreDef.name.toLowerCase()}::vm_impl<arch::${coreDef.name.toLowerCase()}>(*cpu, false);
-            if (port != 0) debugger::server<debugger::gdb_session>::run_server(vm, port);
+		    auto vm = new interp::${coreDef.name.toLowerCase()}::vm_impl<arch::${coreDef.name.toLowerCase()}>(*cpu, false);
+		    if (port != 0) debugger::server<debugger::gdb_session>::run_server(vm, port);
+            if(init_data){
+                auto* cb = reinterpret_cast<std::function<void(arch_if*, arch::traits<arch::${coreDef.name.toLowerCase()}>::reg_t, arch::traits<arch::${coreDef.name.toLowerCase()}>::reg_t)>*>(init_data);
+                cpu->set_semihosting_callback(*cb);
+            }
             return {cpu_ptr{cpu}, vm_ptr{vm}};
         })
 };
diff --git a/gen_input/templates/interp/CORENAME.cpp.gtl b/gen_input/templates/interp/CORENAME.cpp.gtl
index c9d0c8d..099b638 100644
--- a/gen_input/templates/interp/CORENAME.cpp.gtl
+++ b/gen_input/templates/interp/CORENAME.cpp.gtl
@@ -367,12 +367,20 @@ volatile std::array<bool, 2> dummy = {
             auto* cpu = new iss::arch::riscv_hart_m_p<iss::arch::${coreDef.name.toLowerCase()}>();
 		    auto vm = new interp::${coreDef.name.toLowerCase()}::vm_impl<arch::${coreDef.name.toLowerCase()}>(*cpu, false);
 		    if (port != 0) debugger::server<debugger::gdb_session>::run_server(vm, port);
+            if(init_data){
+                auto* cb = reinterpret_cast<std::function<void(arch_if*, arch::traits<arch::${coreDef.name.toLowerCase()}>::reg_t, arch::traits<arch::${coreDef.name.toLowerCase()}>::reg_t)>*>(init_data);
+                cpu->set_semihosting_callback(*cb);
+            }
             return {cpu_ptr{cpu}, vm_ptr{vm}};
         }),
         core_factory::instance().register_creator("${coreDef.name.toLowerCase()}|mu_p|interp", [](unsigned port, void*) -> std::tuple<cpu_ptr, vm_ptr>{
             auto* cpu = new iss::arch::riscv_hart_mu_p<iss::arch::${coreDef.name.toLowerCase()}>();
 		    auto vm = new interp::${coreDef.name.toLowerCase()}::vm_impl<arch::${coreDef.name.toLowerCase()}>(*cpu, false);
 		    if (port != 0) debugger::server<debugger::gdb_session>::run_server(vm, port);
+            if(init_data){
+                auto* cb = reinterpret_cast<std::function<void(arch_if*, arch::traits<arch::${coreDef.name.toLowerCase()}>::reg_t, arch::traits<arch::${coreDef.name.toLowerCase()}>::reg_t)>*>(init_data);
+                cpu->set_semihosting_callback(*cb);
+            }
             return {cpu_ptr{cpu}, vm_ptr{vm}};
         })
 };
diff --git a/gen_input/templates/llvm/CORENAME.cpp.gtl b/gen_input/templates/llvm/CORENAME.cpp.gtl
index 5945e35..da04ae1 100644
--- a/gen_input/templates/llvm/CORENAME.cpp.gtl
+++ b/gen_input/templates/llvm/CORENAME.cpp.gtl
@@ -365,16 +365,24 @@ std::unique_ptr<vm_if> create<arch::${coreDef.name.toLowerCase()}>(arch::${coreD
 namespace iss {
 namespace {
 volatile std::array<bool, 2> dummy = {
-        core_factory::instance().register_creator("${coreDef.name.toLowerCase()}|m_p|llvm", [](unsigned port, void*) -> std::tuple<cpu_ptr, vm_ptr>{
+        core_factory::instance().register_creator("${coreDef.name.toLowerCase()}|m_p|interp", [](unsigned port, void*) -> std::tuple<cpu_ptr, vm_ptr>{
             auto* cpu = new iss::arch::riscv_hart_m_p<iss::arch::${coreDef.name.toLowerCase()}>();
-            auto* vm = new llvm::${coreDef.name.toLowerCase()}::vm_impl<arch::${coreDef.name.toLowerCase()}>(*cpu, false);
-            if (port != 0) debugger::server<debugger::gdb_session>::run_server(vm, port);
+		    auto vm = new interp::${coreDef.name.toLowerCase()}::vm_impl<arch::${coreDef.name.toLowerCase()}>(*cpu, false);
+		    if (port != 0) debugger::server<debugger::gdb_session>::run_server(vm, port);
+            if(init_data){
+                auto* cb = reinterpret_cast<std::function<void(arch_if*, arch::traits<arch::${coreDef.name.toLowerCase()}>::reg_t, arch::traits<arch::${coreDef.name.toLowerCase()}>::reg_t)>*>(init_data);
+                cpu->set_semihosting_callback(*cb);
+            }
             return {cpu_ptr{cpu}, vm_ptr{vm}};
         }),
-        core_factory::instance().register_creator("${coreDef.name.toLowerCase()}|mu_p|llvm", [](unsigned port, void*) -> std::tuple<cpu_ptr, vm_ptr>{
+        core_factory::instance().register_creator("${coreDef.name.toLowerCase()}|mu_p|interp", [](unsigned port, void*) -> std::tuple<cpu_ptr, vm_ptr>{
             auto* cpu = new iss::arch::riscv_hart_mu_p<iss::arch::${coreDef.name.toLowerCase()}>();
-            auto* vm = new llvm::${coreDef.name.toLowerCase()}::vm_impl<arch::${coreDef.name.toLowerCase()}>(*cpu, false);
-            if (port != 0) debugger::server<debugger::gdb_session>::run_server(vm, port);
+		    auto vm = new interp::${coreDef.name.toLowerCase()}::vm_impl<arch::${coreDef.name.toLowerCase()}>(*cpu, false);
+		    if (port != 0) debugger::server<debugger::gdb_session>::run_server(vm, port);
+            if(init_data){
+                auto* cb = reinterpret_cast<std::function<void(arch_if*, arch::traits<arch::${coreDef.name.toLowerCase()}>::reg_t, arch::traits<arch::${coreDef.name.toLowerCase()}>::reg_t)>*>(init_data);
+                cpu->set_semihosting_callback(*cb);
+            }
             return {cpu_ptr{cpu}, vm_ptr{vm}};
         })
 };
diff --git a/src/iss/arch/riscv_hart_m_p.h b/src/iss/arch/riscv_hart_m_p.h
index e22c970..99a7f06 100644
--- a/src/iss/arch/riscv_hart_m_p.h
+++ b/src/iss/arch/riscv_hart_m_p.h
@@ -55,6 +55,8 @@
 #include <util/ities.h>
 #include <util/sparse_array.h>
 
+#include <iss/semihosting/semihosting.h>
+
 #if defined(__GNUC__)
 #define likely(x) __builtin_expect(!!(x), 1)
 #define unlikely(x) __builtin_expect(!!(x), 0)
@@ -290,6 +292,8 @@ public:
 
     void set_irq_num(unsigned i) { mcause_max_irq = 1 << util::ilog2(i); }
 
+    void set_semihosting_callback(std::function<void(arch_if*, reg_t, reg_t)>& cb) { semihosting_cb = cb; };
+
 protected:
     struct riscv_instrumentation_if : public iss::instrumentation_if {
 
@@ -344,9 +348,11 @@ protected:
     reg_t fault_data;
     uint64_t tohost = tohost_dflt;
     uint64_t fromhost = fromhost_dflt;
-    unsigned to_host_wr_cnt = 0;
+    bool tohost_lower_written = false;
     riscv_instrumentation_if instr_if;
 
+    std::function<void(arch_if*, reg_t, reg_t)> semihosting_cb;
+
     using mem_type = util::sparse_array<uint8_t, 1ULL << 32>;
     using csr_type = util::sparse_array<typename traits<BASE>::reg_t, 1ULL << 12, 12>;
     using csr_page_type = typename csr_type::page_type;
@@ -419,6 +425,7 @@ protected:
     feature_config cfg;
     unsigned mcause_max_irq{(FEAT & features_e::FEAT_CLIC) ? std::max(16U, static_cast<unsigned>(traits<BASE>::CLIC_NUM_IRQ)) : 16U};
     inline bool debug_mode_active() { return this->reg.PRIV & 0x4; }
+
     std::pair<std::function<mem_read_f>, std::function<mem_write_f>> replace_mem_access(std::function<mem_read_f> rd,
                                                                                         std::function<mem_write_f> wr) {
         std::pair<std::function<mem_read_f>, std::function<mem_write_f>> ret{hart_mem_rd_delegate, hart_mem_wr_delegate};
@@ -784,7 +791,7 @@ iss::status riscv_hart_m_p<BASE, FEAT>::write(const address_type type, const acc
                     res = write_mem(phys_addr, length, data);
                 }
                 if(unlikely(res != iss::Ok && (access & access_type::DEBUG) == 0)) {
-                    this->reg.trap_state = (1UL << 31) | (7 << 16); // issue trap 7 (Store/AMO access fault)
+                    this->reg.trap_state = (1UL << 31) | (7UL << 16); // issue trap 7 (Store/AMO access fault)
                     fault_data = addr;
                 }
                 return res;
@@ -1098,6 +1105,7 @@ iss::status riscv_hart_m_p<BASE, FEAT>::read_mem(phys_addr_t paddr, unsigned len
 template <typename BASE, features_e FEAT>
 iss::status riscv_hart_m_p<BASE, FEAT>::write_mem(phys_addr_t paddr, unsigned length, const uint8_t* const data) {
     switch(paddr.val) {
+    // TODO remove UART, Peripherals should not be part of the ISS
     case 0xFFFF0000: // UART0 base, TXFIFO reg
         if(((char)data[0]) == '\n' || data[0] == 0) {
             LOG(INFO) << "UART" << ((paddr.val >> 12) & 0x3) << " send '" << uart_buf.str() << "'";
@@ -1115,7 +1123,8 @@ iss::status riscv_hart_m_p<BASE, FEAT>::write_mem(phys_addr_t paddr, unsigned le
             auto tohost_lower = (traits<BASE>::XLEN == 32 && paddr.val == tohost) || (traits<BASE>::XLEN == 64 && paddr.val == tohost);
             if(tohost_lower || tohost_upper) {
                 uint64_t hostvar = *reinterpret_cast<uint64_t*>(p.data() + (tohost & mem.page_addr_mask));
-                if(tohost_upper || (tohost_lower && to_host_wr_cnt > 0)) {
+                // in case of 32 bit system, two writes to tohost are needed, only evaluate on the second (high) write
+                if(tohost_upper && (tohost_lower || tohost_lower_written)) {
                     switch(hostvar >> 48) {
                     case 0:
                         if(hostvar != 0x1) {
@@ -1138,13 +1147,13 @@ iss::status riscv_hart_m_p<BASE, FEAT>::write_mem(phys_addr_t paddr, unsigned le
                             uart_buf.str("");
                         } else
                             uart_buf << c;
-                        to_host_wr_cnt = 0;
                     } break;
                     default:
                         break;
                     }
+                    tohost_lower_written = false;
                 } else if(tohost_lower)
-                    to_host_wr_cnt++;
+                    tohost_lower_written = true;
             } else if((traits<BASE>::XLEN == 32 && paddr.val == fromhost + 4) || (traits<BASE>::XLEN == 64 && paddr.val == fromhost)) {
                 uint64_t fhostvar = *reinterpret_cast<uint64_t*>(p.data() + (fromhost & mem.page_addr_mask));
                 *reinterpret_cast<uint64_t*>(p.data() + (tohost & mem.page_addr_mask)) = fhostvar;
@@ -1253,6 +1262,31 @@ template <typename BASE, features_e FEAT> uint64_t riscv_hart_m_p<BASE, FEAT>::e
             } else {
                 csr[mtval] = addr;
             }
+            if(semihosting_cb) {
+                // Check for semihosting call
+                phys_addr_t p_addr(access_type::DEBUG_READ, traits<BASE>::MEM, addr - 4);
+                std::array<uint8_t, 8> data;
+                // check for SLLI_X0_X0_0X1F and SRAI_X0_X0_0X07
+                this->read_mem(p_addr, 4, data.data());
+                p_addr.val += 8;
+                this->read_mem(p_addr, 4, data.data() + 4);
+
+                const std::array<uint8_t, 8> ref_data = {0x13, 0x10, 0xf0, 0x01, 0x13, 0x50, 0x70, 0x40};
+                if(data == ref_data) {
+                    this->reg.NEXT_PC = addr + 8;
+
+                    std::array<char, 32> buffer;
+#if defined(_MSC_VER)
+                    sprintf(buffer.data(), "0x%016llx", addr);
+#else
+                    sprintf(buffer.data(), "0x%016lx", addr);
+#endif
+                    CLOG(INFO, disass) << "Semihosting call at address " << buffer.data() << " occurred ";
+
+                    semihosting_callback(this, this->reg.X10 /*a0*/, this->reg.X11 /*a1*/);
+                    return this->reg.NEXT_PC;
+                }
+            }
             break;
         case 4:
         case 6:
diff --git a/src/iss/arch/riscv_hart_msu_vp.h b/src/iss/arch/riscv_hart_msu_vp.h
index 2a4bc90..4329160 100644
--- a/src/iss/arch/riscv_hart_msu_vp.h
+++ b/src/iss/arch/riscv_hart_msu_vp.h
@@ -55,6 +55,8 @@
 #include <util/ities.h>
 #include <util/sparse_array.h>
 
+#include <iss/semihosting/semihosting.h>
+
 #if defined(__GNUC__)
 #define likely(x) __builtin_expect(!!(x), 1)
 #define unlikely(x) __builtin_expect(!!(x), 0)
@@ -341,6 +343,8 @@ public:
 
     void set_irq_num(unsigned i) { mcause_max_irq = 1 << util::ilog2(i); }
 
+    void set_semihosting_callback(std::function<void(arch_if*, reg_t, reg_t)>& cb) { semihosting_cb = cb; };
+
 protected:
     struct riscv_instrumentation_if : public iss::instrumentation_if {
 
@@ -395,9 +399,11 @@ protected:
     std::array<vm_info, 2> vm;
     uint64_t tohost = tohost_dflt;
     uint64_t fromhost = fromhost_dflt;
-    unsigned to_host_wr_cnt = 0;
+    bool tohost_lower_written = false;
     riscv_instrumentation_if instr_if;
 
+    std::function<void(arch_if*, reg_t, reg_t)> semihosting_cb;
+
     using mem_type = util::sparse_array<uint8_t, 1ULL << 32>;
     using csr_type = util::sparse_array<typename traits<BASE>::reg_t, 1ULL << 12, 12>;
     using csr_page_type = typename csr_type::page_type;
@@ -1092,7 +1098,8 @@ template <typename BASE> iss::status riscv_hart_msu_vp<BASE>::write_mem(phys_add
             auto tohost_lower = (traits<BASE>::XLEN == 32 && paddr.val == tohost) || (traits<BASE>::XLEN == 64 && paddr.val == tohost);
             if(tohost_lower || tohost_upper) {
                 uint64_t hostvar = *reinterpret_cast<uint64_t*>(p.data() + (tohost & mem.page_addr_mask));
-                if(tohost_upper || (tohost_lower && to_host_wr_cnt > 0)) {
+                // in case of 32 bit system, two writes to tohost are needed, only evaluate on the second (high) write
+                if(tohost_upper && (tohost_lower || tohost_lower_written)) {
                     switch(hostvar >> 48) {
                     case 0:
                         if(hostvar != 0x1) {
@@ -1104,8 +1111,10 @@ template <typename BASE> iss::status riscv_hart_msu_vp<BASE>::write_mem(phys_add
                         }
                         this->reg.trap_state = std::numeric_limits<uint32_t>::max();
                         this->interrupt_sim = hostvar;
+#ifndef WITH_TCC
+                        throw(iss::simulation_stopped(hostvar));
+#endif
                         break;
-                        // throw(iss::simulation_stopped(hostvar));
                     case 0x0101: {
                         char c = static_cast<char>(hostvar & 0xff);
                         if(c == '\n' || c == 0) {
@@ -1113,13 +1122,13 @@ template <typename BASE> iss::status riscv_hart_msu_vp<BASE>::write_mem(phys_add
                             uart_buf.str("");
                         } else
                             uart_buf << c;
-                        to_host_wr_cnt = 0;
                     } break;
                     default:
                         break;
                     }
+                    tohost_lower_written = false;
                 } else if(tohost_lower)
-                    to_host_wr_cnt++;
+                    tohost_lower_written = true;
             } else if((traits<BASE>::XLEN == 32 && paddr.val == fromhost + 4) || (traits<BASE>::XLEN == 64 && paddr.val == fromhost)) {
                 uint64_t fhostvar = *reinterpret_cast<uint64_t*>(p.data() + (fromhost & mem.page_addr_mask));
                 *reinterpret_cast<uint64_t*>(p.data() + (tohost & mem.page_addr_mask)) = fhostvar;
@@ -1304,6 +1313,31 @@ template <typename BASE> uint64_t riscv_hart_msu_vp<BASE>::enter_trap(uint64_t f
             // csr[dpc] = addr;
             // csr[dcsr] = (csr[dcsr] & ~0x1c3) | (1<<6) | PRIV_M; //FIXME: cause should not be 4 (stepi)
             csr[utval | (new_priv << 8)] = addr;
+            if(semihosting_cb) {
+                // Check for semihosting call
+                phys_addr_t p_addr(access_type::DEBUG_READ, traits<BASE>::MEM, addr - 4);
+                std::array<uint8_t, 8> data;
+                // check for SLLI_X0_X0_0X1F and SRAI_X0_X0_0X07
+                this->read_mem(p_addr, 4, data.data());
+                p_addr.val += 8;
+                this->read_mem(p_addr, 4, data.data() + 4);
+
+                const std::array<uint8_t, 8> ref_data = {0x13, 0x10, 0xf0, 0x01, 0x13, 0x50, 0x70, 0x40};
+                if(data == ref_data) {
+                    this->reg.NEXT_PC = addr + 8;
+
+                    std::array<char, 32> buffer;
+#if defined(_MSC_VER)
+                    sprintf(buffer.data(), "0x%016llx", addr);
+#else
+                    sprintf(buffer.data(), "0x%016lx", addr);
+#endif
+                    CLOG(INFO, disass) << "Semihosting call at address " << buffer.data() << " occurred ";
+
+                    semihosting_callback(this, this->reg.X10 /*a0*/, this->reg.X11 /*a1*/);
+                    return this->reg.NEXT_PC;
+                }
+            }
             break;
         case 4:
         case 6:
@@ -1321,7 +1355,7 @@ template <typename BASE> uint64_t riscv_hart_msu_vp<BASE>::enter_trap(uint64_t f
         this->reg.pending_trap = 0;
     }
     size_t adr = ucause | (new_priv << 8);
-    csr[adr] = (trap_id << 31) + cause;
+    csr[adr] = (trap_id << (traits<BASE>::XLEN - 1)) + cause;
     // update mstatus
     // xPP field of mstatus is written with the active privilege mode at the time
     // of the trap; the x PIE field of mstatus
diff --git a/src/iss/arch/riscv_hart_mu_p.h b/src/iss/arch/riscv_hart_mu_p.h
index 7aa8b21..362fece 100644
--- a/src/iss/arch/riscv_hart_mu_p.h
+++ b/src/iss/arch/riscv_hart_mu_p.h
@@ -55,6 +55,8 @@
 #include <util/ities.h>
 #include <util/sparse_array.h>
 
+#include <iss/semihosting/semihosting.h>
+
 #if defined(__GNUC__)
 #define likely(x) __builtin_expect(!!(x), 1)
 #define unlikely(x) __builtin_expect(!!(x), 0)
@@ -317,6 +319,8 @@ public:
 
     void set_irq_num(unsigned i) { mcause_max_irq = 1 << util::ilog2(i); }
 
+    void set_semihosting_callback(std::function<void(arch_if*, reg_t, reg_t)>& cb) { semihosting_cb = cb; };
+
 protected:
     struct riscv_instrumentation_if : public iss::instrumentation_if {
 
@@ -371,9 +375,11 @@ protected:
     reg_t fault_data;
     uint64_t tohost = tohost_dflt;
     uint64_t fromhost = fromhost_dflt;
-    unsigned to_host_wr_cnt = 0;
+    bool tohost_lower_written = false;
     riscv_instrumentation_if instr_if;
 
+    std::function<void(arch_if*, reg_t, reg_t)> semihosting_cb;
+
     using mem_type = util::sparse_array<uint8_t, 1ULL << 32>;
     using csr_type = util::sparse_array<typename traits<BASE>::reg_t, 1ULL << 12, 12>;
     using csr_page_type = typename csr_type::page_type;
@@ -1317,6 +1323,7 @@ iss::status riscv_hart_mu_p<BASE, FEAT>::read_mem(phys_addr_t paddr, unsigned le
 template <typename BASE, features_e FEAT>
 iss::status riscv_hart_mu_p<BASE, FEAT>::write_mem(phys_addr_t paddr, unsigned length, const uint8_t* const data) {
     switch(paddr.val) {
+    // TODO remove UART, Peripherals should not be part of the ISS
     case 0xFFFF0000: // UART0 base, TXFIFO reg
         if(((char)data[0]) == '\n' || data[0] == 0) {
             LOG(INFO) << "UART" << ((paddr.val >> 12) & 0x3) << " send '" << uart_buf.str() << "'";
@@ -1334,7 +1341,8 @@ iss::status riscv_hart_mu_p<BASE, FEAT>::write_mem(phys_addr_t paddr, unsigned l
             auto tohost_lower = (traits<BASE>::XLEN == 32 && paddr.val == tohost) || (traits<BASE>::XLEN == 64 && paddr.val == tohost);
             if(tohost_lower || tohost_upper) {
                 uint64_t hostvar = *reinterpret_cast<uint64_t*>(p.data() + (tohost & mem.page_addr_mask));
-                if(tohost_upper || (tohost_lower && to_host_wr_cnt > 0)) {
+                // in case of 32 bit system, two writes to tohost are needed, only evaluate on the second (high) write
+                if(tohost_upper && (tohost_lower || tohost_lower_written)) {
                     switch(hostvar >> 48) {
                     case 0:
                         if(hostvar != 0x1) {
@@ -1346,8 +1354,10 @@ iss::status riscv_hart_mu_p<BASE, FEAT>::write_mem(phys_addr_t paddr, unsigned l
                         }
                         this->reg.trap_state = std::numeric_limits<uint32_t>::max();
                         this->interrupt_sim = hostvar;
+#ifndef WITH_TCC
+                        throw(iss::simulation_stopped(hostvar));
+#endif
                         break;
-                        // throw(iss::simulation_stopped(hostvar));
                     case 0x0101: {
                         char c = static_cast<char>(hostvar & 0xff);
                         if(c == '\n' || c == 0) {
@@ -1355,13 +1365,13 @@ iss::status riscv_hart_mu_p<BASE, FEAT>::write_mem(phys_addr_t paddr, unsigned l
                             uart_buf.str("");
                         } else
                             uart_buf << c;
-                        to_host_wr_cnt = 0;
                     } break;
                     default:
                         break;
                     }
+                    tohost_lower_written = false;
                 } else if(tohost_lower)
-                    to_host_wr_cnt++;
+                    tohost_lower_written = true;
             } else if((traits<BASE>::XLEN == 32 && paddr.val == fromhost + 4) || (traits<BASE>::XLEN == 64 && paddr.val == fromhost)) {
                 uint64_t fhostvar = *reinterpret_cast<uint64_t*>(p.data() + (fromhost & mem.page_addr_mask));
                 *reinterpret_cast<uint64_t*>(p.data() + (tohost & mem.page_addr_mask)) = fhostvar;
@@ -1474,6 +1484,31 @@ template <typename BASE, features_e FEAT> uint64_t riscv_hart_mu_p<BASE, FEAT>::
             } else {
                 csr[utval | (new_priv << 8)] = addr;
             }
+            if(semihosting_cb) {
+                // Check for semihosting call
+                phys_addr_t p_addr(access_type::DEBUG_READ, traits<BASE>::MEM, addr - 4);
+                std::array<uint8_t, 8> data;
+                // check for SLLI_X0_X0_0X1F and SRAI_X0_X0_0X07
+                this->read_mem(p_addr, 4, data.data());
+                p_addr.val += 8;
+                this->read_mem(p_addr, 4, data.data() + 4);
+
+                const std::array<uint8_t, 8> ref_data = {0x13, 0x10, 0xf0, 0x01, 0x13, 0x50, 0x70, 0x40};
+                if(data == ref_data) {
+                    this->reg.NEXT_PC = addr + 8;
+
+                    std::array<char, 32> buffer;
+#if defined(_MSC_VER)
+                    sprintf(buffer.data(), "0x%016llx", addr);
+#else
+                    sprintf(buffer.data(), "0x%016lx", addr);
+#endif
+                    CLOG(INFO, disass) << "Semihosting call at address " << buffer.data() << " occurred ";
+
+                    semihosting_callback(this, this->reg.X10 /*a0*/, this->reg.X11 /*a1*/);
+                    return this->reg.NEXT_PC;
+                }
+            }
             break;
         case 4:
         case 6:
diff --git a/src/iss/arch/tgc5c.h b/src/iss/arch/tgc5c.h
index 7285edd..36254f7 100644
--- a/src/iss/arch/tgc5c.h
+++ b/src/iss/arch/tgc5c.h
@@ -201,7 +201,6 @@ struct tgc5c: public arch_if {
 
     inline uint32_t get_last_branch() { return reg.last_branch; }
 
-
 #pragma pack(push, 1)
     struct TGC5C_regs { 
         uint32_t X0 = 0; 
diff --git a/src/iss/semihosting/semihosting.cpp b/src/iss/semihosting/semihosting.cpp
new file mode 100644
index 0000000..183ae03
--- /dev/null
+++ b/src/iss/semihosting/semihosting.cpp
@@ -0,0 +1,22 @@
+#include "semihosting.h"
+#include <exception>
+#include <iss/vm_types.h>
+#include <stdexcept>
+template <typename T> void semihosting_callback(iss::arch_if* arch_if_ptr, T a0, T a1) {
+    if(a0 == 0x04 /*WRITE0*/) {
+        uint8_t character;
+        while(1) {
+            auto res = arch_if_ptr->read(iss::address_type::PHYSICAL, iss::access_type::DEBUG_READ, 0, a1, 1, &character);
+            if(res != iss::Ok)
+                return;
+            if(character == 0)
+                break;
+            putchar(character);
+            a1++;
+        }
+    } else {
+        throw std::runtime_error("Not Implemented Error");
+    }
+}
+template void semihosting_callback<uint32_t>(iss::arch_if* arch_if_ptr, uint32_t a0, uint32_t a1);
+template void semihosting_callback<uint64_t>(iss::arch_if* arch_if_ptr, uint64_t a0, uint64_t a1);
diff --git a/src/iss/semihosting/semihosting.h b/src/iss/semihosting/semihosting.h
new file mode 100644
index 0000000..cf396e4
--- /dev/null
+++ b/src/iss/semihosting/semihosting.h
@@ -0,0 +1,6 @@
+#ifndef _SEMIHOSTING_H_
+#define _SEMIHOSTING_H_
+#include <iss/arch_if.h>
+template <typename T> void semihosting_callback(iss::arch_if* arch_if_ptr, T a0, T a1);
+
+#endif
\ No newline at end of file
diff --git a/src/main.cpp b/src/main.cpp
index 61f3947..aad92da 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -31,8 +31,10 @@
  *******************************************************************************/
 
 #include <array>
+#include <cstdint>
 #include <iostream>
 #include <iss/factory.h>
+#include <iss/semihosting/semihosting.h>
 #include <vector>
 
 #include "iss/arch/tgc_mapper.h"
@@ -52,7 +54,6 @@
 #endif
 
 namespace po = boost::program_options;
-
 int main(int argc, char* argv[]) {
     /*
      *  Define and parse the program options
@@ -116,6 +117,7 @@ int main(int argc, char* argv[]) {
         // instantiate the simulator
         iss::vm_ptr vm{nullptr};
         iss::cpu_ptr cpu{nullptr};
+        std::function<void(iss::arch_if*, uint32_t, uint32_t)> semihosting_cb = &semihosting_callback<uint32_t>;
         std::string isa_opt(clim["isa"].as<std::string>());
         if(isa_opt.size() == 0 || isa_opt == "?") {
             auto list = f.get_names();
@@ -123,7 +125,8 @@ int main(int argc, char* argv[]) {
             std::cout << "Available implementations (core|platform|backend):\n  - " << util::join(list, "\n  - ") << std::endl;
             return 0;
         } else if(isa_opt.find('|') != std::string::npos) {
-            std::tie(cpu, vm) = f.create(isa_opt + "|" + clim["backend"].as<std::string>(), clim["gdb-port"].as<unsigned>());
+            std::tie(cpu, vm) =
+                f.create(isa_opt + "|" + clim["backend"].as<std::string>(), clim["gdb-port"].as<unsigned>(), &semihosting_cb);
         } else {
             auto base_isa = isa_opt.substr(0, 5);
             if(base_isa == "tgc5d" || base_isa == "tgc5e") {
@@ -131,7 +134,7 @@ int main(int argc, char* argv[]) {
             } else {
                 isa_opt += "|m_p|" + clim["backend"].as<std::string>();
             }
-            std::tie(cpu, vm) = f.create(isa_opt, clim["gdb-port"].as<unsigned>());
+            std::tie(cpu, vm) = f.create(isa_opt, clim["gdb-port"].as<unsigned>(), &semihosting_cb);
         }
         if(!cpu) {
             LOG(ERR) << "Could not create cpu for isa " << isa_opt << " and backend " << clim["backend"].as<std::string>() << std::endl;
diff --git a/src/vm/interp/vm_tgc5c.cpp b/src/vm/interp/vm_tgc5c.cpp
index 99419ac..10b6296 100644
--- a/src/vm/interp/vm_tgc5c.cpp
+++ b/src/vm/interp/vm_tgc5c.cpp
@@ -2695,16 +2695,24 @@ std::unique_ptr<vm_if> create<arch::tgc5c>(arch::tgc5c *core, unsigned short por
 namespace iss {
 namespace {
 volatile std::array<bool, 2> dummy = {
-        core_factory::instance().register_creator("tgc5c|m_p|interp", [](unsigned port, void*) -> std::tuple<cpu_ptr, vm_ptr>{
+        core_factory::instance().register_creator("tgc5c|m_p|interp", [](unsigned port, void* init_data) -> std::tuple<cpu_ptr, vm_ptr>{
             auto* cpu = new iss::arch::riscv_hart_m_p<iss::arch::tgc5c>();
 		    auto vm = new interp::tgc5c::vm_impl<arch::tgc5c>(*cpu, false);
 		    if (port != 0) debugger::server<debugger::gdb_session>::run_server(vm, port);
+            if(init_data){
+                auto* cb = reinterpret_cast<std::function<void(arch_if*, arch::traits<arch::tgc5c>::reg_t, arch::traits<arch::tgc5c>::reg_t)>*>(init_data);
+                cpu->set_semihosting_callback(*cb);
+            }
             return {cpu_ptr{cpu}, vm_ptr{vm}};
         }),
-        core_factory::instance().register_creator("tgc5c|mu_p|interp", [](unsigned port, void*) -> std::tuple<cpu_ptr, vm_ptr>{
+        core_factory::instance().register_creator("tgc5c|mu_p|interp", [](unsigned port, void* init_data) -> std::tuple<cpu_ptr, vm_ptr>{
             auto* cpu = new iss::arch::riscv_hart_mu_p<iss::arch::tgc5c>();
 		    auto vm = new interp::tgc5c::vm_impl<arch::tgc5c>(*cpu, false);
 		    if (port != 0) debugger::server<debugger::gdb_session>::run_server(vm, port);
+            if(init_data){
+                auto* cb = reinterpret_cast<std::function<void(arch_if*, arch::traits<arch::tgc5c>::reg_t, arch::traits<arch::tgc5c>::reg_t)>*>(init_data);
+                cpu->set_semihosting_callback(*cb);
+            }
             return {cpu_ptr{cpu}, vm_ptr{vm}};
         })
 };