diff --git a/demo/helloworld/Makefile b/demo/helloworld/Makefile
index fae92cc..82b6d1f 100644
--- a/demo/helloworld/Makefile
+++ b/demo/helloworld/Makefile
@@ -2,10 +2,6 @@ export RV_ROOT = ${PWD}/../..
 LLVMINSTALL = /home/colin/develop/llvm-build/install
 GCC_PREFIX = /opt/riscv/bin/riscv64-unknown-elf
 
-
-TEST_CFLAGS = -g -O3 -funroll-all-loops
-ABI = -mabi=ilp32 -march=rv32imc
-
 DEMODIR = ${PWD}
 BUILD_DIR = ${DEMODIR}/build
 RV_DESIGN = ${RV_ROOT}/design
@@ -20,10 +16,6 @@ endif
 
 LINK = $(DEMODIR)/link.ld
 
-OFILES = $(TEST).o
-OUTFILES = $(TEST).out
-
-
 # CFLAGS for verilator generated Makefiles. Without -std=c++11 it complains for `auto` variables
 CFLAGS += "-std=c++11"
 # Optimization for better performance; alternative is nothing for slower runtime (faster compiles)
@@ -60,25 +52,12 @@ verilator: program.hex verilator-build
 
 ##################### Test hex Build #####################################
 
-# program.hex: $(OFILES) $(LINK)
-# 	@echo Building $(TEST)
-# 	$(GCC_PREFIX)-gcc $(ABI) -Wl,-Map=$(BUILD_DIR)/$(TEST).map -lgcc -T$(LINK) -o $(BUILD_DIR)/$(TEST).bin $(BUILD_DIR)/$(OFILES) -nostartfiles  $(TEST_LIBS)
-# 	$(GCC_PREFIX)-objcopy -O verilog $(BUILD_DIR)/$(TEST).bin $(BUILD_DIR)/program.hex
-# 	$(GCC_PREFIX)-objdump -S  $(BUILD_DIR)/$(TEST).bin > $(BUILD_DIR)/$(TEST).dis
-# 	@echo Completed building $(TEST)
-
-# %.o : %.s swerv_define
-# 	$(GCC_PREFIX)-cpp -I${BUILD_DIR}  $<  > $(BUILD_DIR)/$*.cpp.s
-# 	$(GCC_PREFIX)-as $(ABI) $(BUILD_DIR)/$*.cpp.s -o $(BUILD_DIR)/$@
-
-program.hex: $(OUTFILES)
+program.hex: $(TEST).out
 	@echo Building $(TEST)
 	$(GCC_PREFIX)-objcopy -O verilog $(BUILD_DIR)/$(TEST).out $(BUILD_DIR)/program.hex
 	$(GCC_PREFIX)-objdump -S  $(BUILD_DIR)/$(TEST).out > $(BUILD_DIR)/$(TEST).dis
 
-##################### llvm hex Build #####################################
-
-build: $(OUTFILES)
+build: $(TEST).out
 	@echo Completed building $(TEST)
 
 %.out : %.c swerv_define
diff --git a/demo/jtag/Makefile b/demo/jtag/Makefile
new file mode 100644
index 0000000..cea4840
--- /dev/null
+++ b/demo/jtag/Makefile
@@ -0,0 +1,75 @@
+export RV_ROOT = ${PWD}/../..
+LLVMINSTALL = /home/colin/develop/llvm-build/install
+GCC_PREFIX = /opt/riscv/bin/riscv64-unknown-elf
+
+
+TEST_CFLAGS = -g -O3 -funroll-all-loops
+ABI = -mabi=ilp32 -march=rv32imc
+
+DEMODIR = ${PWD}
+BUILD_DIR = ${DEMODIR}/build
+RV_DESIGN = ${RV_ROOT}/design
+RV_SOC = ${RV_ROOT}/soc
+
+TEST = jtag
+
+ifdef debug
+ DEBUG_PLUS = +dumpon
+ VERILATOR_DEBUG = --trace
+endif
+
+LINK = $(DEMODIR)/link.ld
+
+# CFLAGS for verilator generated Makefiles. Without -std=c++11 it complains for `auto` variables
+CFLAGS += "-std=c++11"
+# Optimization for better performance; alternative is nothing for slower runtime (faster compiles)
+# -O2 for faster runtime (slower compiles), or -O for balance.
+VERILATOR_MAKE_FLAGS = OPT_FAST="-Os"
+
+# Targets
+all: clean verilator
+
+clean:
+	rm -rf build obj_dir
+
+swerv_define :
+	BUILD_PATH=${BUILD_DIR} PERLLIB=${RV_SOC} ${RV_SOC}/swerv.config -target=default -set iccm_enable
+
+##################### Verilog Builds #####################################
+
+verilator-build: swerv_define
+	echo '`undef ASSERT_ON' >> ${BUILD_DIR}/common_defines.vh
+	verilator --cc -CFLAGS ${CFLAGS} \
+	              $(BUILD_DIR)/common_defines.vh \
+	              -I${BUILD_DIR} \
+	              -Wno-UNOPTFLAT \
+	              -F ${RV_SOC}/soc.mk \
+	              $(RV_SOC)/soc_top.sv \
+	              --top-module soc_top -exe test_soc_top.cpp --autoflush $(VERILATOR_DEBUG)
+	cp ${DEMODIR}/test_soc_top.cpp obj_dir
+	$(MAKE) -j -C obj_dir/ -f Vsoc_top.mk $(VERILATOR_MAKE_FLAGS)
+
+##################### Simulation Runs #####################################
+
+verilator: program.hex verilator-build
+	cd build && ../obj_dir/Vsoc_top ${DEBUG_PLUS}
+
+##################### Test hex Build #####################################
+
+program.hex: $(TEST).o $(LINK)
+	@echo Building $(TEST)
+	$(GCC_PREFIX)-gcc $(ABI) -Wl,-Map=$(BUILD_DIR)/$(TEST).map -lgcc -T$(LINK) -o $(BUILD_DIR)/$(TEST).bin $(BUILD_DIR)/$(TEST).o -nostartfiles  $(TEST_LIBS)
+	$(GCC_PREFIX)-objcopy -O verilog $(BUILD_DIR)/$(TEST).bin $(BUILD_DIR)/program.hex
+	$(GCC_PREFIX)-objdump -S  $(BUILD_DIR)/$(TEST).bin > $(BUILD_DIR)/$(TEST).dis
+	@echo Completed building $(TEST)
+
+%.o : %.s swerv_define
+	$(GCC_PREFIX)-cpp -I${BUILD_DIR}  $<  > $(BUILD_DIR)/$*.cpp.s
+	$(GCC_PREFIX)-as $(ABI) $(BUILD_DIR)/$*.cpp.s -o $(BUILD_DIR)/$@
+
+##################### llvm hex Build #####################################
+
+help:
+	@echo Possible targets: verilator help clean all verilator-build program.hex
+
+.PHONY: help clean verilator
\ No newline at end of file
diff --git a/demo/jtag/jtag.s b/demo/jtag/jtag.s
new file mode 100644
index 0000000..3a200d2
--- /dev/null
+++ b/demo/jtag/jtag.s
@@ -0,0 +1,81 @@
+// SPDX-License-Identifier: Apache-2.0
+// Copyright 2019 Western Digital Corporation or its affiliates.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+
+// Assembly code for Hello World
+// Not using only ALU ops for creating the string
+
+
+#include "defines.h"
+
+#define STDOUT 0xd0580000
+
+
+// Code to execute
+.section .text
+.global _start
+_start:
+
+    // Clear minstret
+    csrw minstret, zero
+    csrw minstreth, zero
+
+    // Set up MTVEC - not expecting to use it though
+    li x1, RV_ICCM_SADR
+    csrw mtvec, x1
+
+
+    // Enable Caches in MRAC
+    li x1, 0x5f555555
+    csrw 0x7c0, x1
+
+    // Load string from hw_data
+    // and write to stdout address
+
+    li x3, STDOUT
+    la x4, hw_data
+
+loop:
+   lb x5, 0(x4)
+   sb x5, 0(x3)
+   addi x4, x4, 1
+   bnez x5, loop
+
+   li x3, STDOUT
+   la x4, hw_data
+
+loop2:
+   lb x5, 0(x4)
+   sb x5, 0(x3)
+   addi x4, x4, 1
+   bnez x5, loop2
+
+// Write 0xff to STDOUT for TB to terminate test.
+_finish:
+    li x3, STDOUT
+    addi x5, x0, 0xff
+    sb x5, 0(x3)
+    beq x0, x0, _finish
+.rept 100
+    nop
+.endr
+
+.global hw_data
+.data
+hw_data:
+.ascii "----------------------------------\n"
+.ascii "Hello World Colin.liang EH1@WDC !!\n"
+.ascii "----------------------------------\n"
+.byte 0
diff --git a/demo/jtag/link.ld b/demo/jtag/link.ld
new file mode 100644
index 0000000..b84cf31
--- /dev/null
+++ b/demo/jtag/link.ld
@@ -0,0 +1,16 @@
+
+OUTPUT_ARCH( "riscv" )
+ENTRY(_start)
+
+SECTIONS
+{
+  . = 0;
+  .text_init   : { *(.text_init*) }
+  .text   : { *(.text*) }
+  _end = .;
+  . = 0x10000;
+  .data  :  ALIGN(0x800) { *(.*data) *(.rodata*) STACK = ALIGN(16) + 0x8000; }
+  .bss : { *(.bss) }
+  . = 0xd0580000;
+  .data.io   : { *(.data.io) }
+}
diff --git a/demo/jtag/test_soc_top.cpp b/demo/jtag/test_soc_top.cpp
new file mode 100644
index 0000000..4295f0b
--- /dev/null
+++ b/demo/jtag/test_soc_top.cpp
@@ -0,0 +1,65 @@
+// SPDX-License-Identifier: Apache-2.0
+// Copyright 2019 Western Digital Corporation or its affiliates.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+#include <stdlib.h>
+#include <iostream>
+#include <utility>
+#include <string>
+#include "Vsoc_top.h"
+#include "verilated.h"
+#include "verilated_vcd_c.h"
+
+
+vluint64_t main_time = 0;
+
+double sc_time_stamp () {
+ return main_time;
+}
+
+
+int main(int argc, char** argv) {
+  std::cout << "\nVerilatorTB: Start of sim\n" << std::endl;
+
+  Verilated::commandArgs(argc, argv);
+
+  Vsoc_top* soc = new Vsoc_top;
+
+  // init trace dump
+  VerilatedVcdC* tfp = NULL;
+
+#if VM_TRACE
+  Verilated::traceEverOn(true);
+  tfp = new VerilatedVcdC;
+  soc->trace (tfp, 24);
+  tfp->open ("sim.vcd");
+#endif
+  // Simulate
+  while(!Verilated::gotFinish()){
+#if VM_TRACE
+      tfp->dump (main_time);
+#endif
+      main_time += 5;
+      soc->core_clk = !soc->core_clk;
+      soc->eval();
+  }
+
+#if VM_TRACE
+  tfp->close();
+#endif
+
+  std::cout << "\nVerilatorTB: End of sim" << std::endl;
+  exit(EXIT_SUCCESS);
+
+}
diff --git a/soc/soc_top.sv b/soc/soc_top.sv
index fa8730c..a05895e 100644
--- a/soc/soc_top.sv
+++ b/soc/soc_top.sv
@@ -447,6 +447,19 @@ module soc_top;
    //=========================================================================-
    // RTL instance
    //=========================================================================-
+
+jtagdpi jtagdpi(
+    .clk_i(core_clk),
+    .rst_ni(rst_l),
+
+    .jtag_tck(jtag_tck),
+    .jtag_tms(jtag_tms),
+    .jtag_tdi(jtag_tdi),
+    .jtag_tdo(jtag_tdo),
+    .jtag_trst_n(jtag_trst_n),
+    .jtag_srst_n()
+  );
+
 swerv_wrapper rvtop (
     .rst_l                  ( rst_l         ),
     .dbg_rst_l              ( porst_l       ),