From: Staf Verhaegen <staf@stafverhaegen.be>
Date: Fri, 6 Dec 2019 18:07:41 +0000 (+0100)
Subject: [broken]Move code
X-Git-Tag: 24jan2021ls180~47
X-Git-Url: https://git.libre-soc.org/?p=c4m-jtag.git;a=commitdiff_plain;h=2af9a2c55b5a5e5e17c484ebfc9bcaabf6cb6781;hp=abf90f14e0f5247c90f9513d0051b90f3def1b87

[broken]Move code

Moved files without any changes to easily track later changes.
---

diff --git a/bench/vhdl/idcode.vhdl b/bench/vhdl/idcode.vhdl
deleted file mode 100644
index 550df98..0000000
--- a/bench/vhdl/idcode.vhdl
+++ /dev/null
@@ -1,102 +0,0 @@
--- reset JTAG interface and then IDCODE should be shifted out
-
-library ieee;
-use ieee.std_logic_1164.ALL;
-
-use work.c4m_jtag.ALL;
-
-entity bench_idcode is
-end bench_idcode;
-
-architecture rtl of bench_idcode is
-  signal TCK:   std_logic;
-  signal TMS:   std_logic;
-  signal TDI:   std_logic;
-  signal TDO:   std_logic;
-  signal TRST_N: std_logic;
-
-  constant CLK_PERIOD:  time := 10 ns;
-
-  procedure ClkCycle(
-    signal CLK: out std_logic;
-    CLK_PERIOD: time
-  ) is
-  begin
-    CLK <= '0';
-    wait for CLK_PERIOD/4;
-    CLK <= '1';
-    wait for CLK_PERIOD/2;
-    CLK <= '0';
-    wait for CLK_PERIOD/4;
-  end ClkCycle;
-
-  procedure ClkCycles(
-    N:  integer;
-    signal CLK: out std_logic;
-    CLK_PERIOD: time
-  ) is
-  begin
-    for i in 1 to N loop
-      ClkCycle(CLK, CLK_PERIOD);
-    end loop;
-  end ClkCycles;
-begin
-  JTAG_BLOCK: c4m_jtag_tap_controller
-    -- Use default values
-    port map (
-      TCK => TCK,
-      TMS => TMS,
-      TDI => TDI,
-      TDO => TDO,
-      TRST_N => TRST_N,
-      RESET => open,
-      DRCAPTURE => open,
-      DRSHIFT => open,
-      DRUPDATE => open,
-      IR => open,
-      CORE_OUT => "0",
-      CORE_IN => open,
-      CORE_EN => "0",
-      PAD_OUT => open,
-      PAD_IN => "0",
-      PAD_EN => open
-    );
-
-  SIM: process
-  begin
-    -- Reset
-    TCK <= '0';
-    TMS <= '1';
-    TDI <= '0';
-    TRST_N <= '0';
-    wait for 10*CLK_PERIOD;
-
-    TRST_N <= '1';
-    wait for CLK_PERIOD;
-
-    -- Enter RunTestIdle
-    TMS <= '0';
-    ClkCycle(TCK, CLK_PERIOD);
-    -- Enter SelectDRScan
-    TMS <= '1';
-    ClkCycle(TCK, CLK_PERIOD);
-    -- Enter Capture
-    TMS <= '0';
-    ClkCycle(TCK, CLK_PERIOD);
-    -- Enter Shift, run for 35 CLK cycles
-    TMS <= '0';
-    ClkCycles(35, TCK, CLK_PERIOD);
-    -- Enter Exit1
-    TMS <= '1';
-    ClkCycle(TCK, CLK_PERIOD);
-    -- Enter Update
-    TMS <= '1';
-    ClkCycle(TCK, CLK_PERIOD);
-    -- To TestLogicReset
-    TMS <= '1';
-    ClkCycles(4, TCK, CLK_PERIOD);
-
-    -- end simulation
-    wait;
-  end process;
-end rtl;
diff --git a/bench/vhdl/sampleshift.vhdl b/bench/vhdl/sampleshift.vhdl
deleted file mode 100644
index 62ec28b..0000000
--- a/bench/vhdl/sampleshift.vhdl
+++ /dev/null
@@ -1,50 +0,0 @@
--- Test JTAG in the following way:
---  * reset JTAG interface
---  * load samplepreload command
---  * shift in/out sampled inputs + wanted outputs
---  * load extest command
---  * execute
-
-
-library ieee;
-use ieee.std_logic_1164.ALL;
-
-use work.c4m_jtag.ALL;
-
-entity bench_sampleshift is
-end bench_sampleshift;
-
-architecture rtl of bench_sampleshift is
-  signal TCK:   std_logic;
-  signal TMS:   std_logic;
-  signal TDI:   std_logic;
-  signal TDO:   std_logic;
-  signal TRST_N: std_logic;
-  
-  constant CLK_PERIOD:  time := 10 ns;
-
-  procedure ClkCycle(
-    signal CLK: out std_logic;
-    CLK_PERIOD: time
-  ) is
-  begin
-    CLK <= '0';
-    wait for CLK_PERIOD/4;
-    CLK <= '1';
-    wait for CLK_PERIOD/2;
-    CLK <= '0';
-    wait for CLK_PERIOD/4;
-  end ClkCycle;
-
-  procedure ClkCycles(
-    N:  integer;
-    signal CLK: out std_logic;
-    CLK_PERIOD: time
-  ) is
-  begin
-    for i in 1 to N loop
-      ClkCycle(CLK, CLK_PERIOD);
-    end loop;
-  end ClkCycles;
-  
-  procedure LoadIR(
diff --git a/c4m/cocotb/jtag/c4m_jtag.py b/c4m/cocotb/jtag/c4m_jtag.py
new file mode 100644
index 0000000..1dd13b1
--- /dev/null
+++ b/c4m/cocotb/jtag/c4m_jtag.py
@@ -0,0 +1,210 @@
+import cocotb
+from cocotb.triggers import Timer
+from cocotb.binary import BinaryValue
+
+class JTAGException(Exception):
+    pass
+
+class JTAG_Clock(object):
+    """
+    Class for the JTAG clock, run cycle by cycle
+    """
+    def __init__(self, signal, period):
+        self.signal = signal
+        self.t = Timer(period/4)
+
+    @cocotb.coroutine
+    def Cycle(self, cycles=1):
+        """
+        Do one or more cycles
+        Cycle start in middle of 0 pulse of the clock
+        """
+        for i in range(cycles):
+            self.signal <= 0
+            yield self.t
+            self.signal <= 1
+            yield self.t
+            yield self.t
+            self.signal <= 0
+            yield self.t
+
+class JTAG_Master(object):
+    """
+    Class that will run JTAG commands, shift in and out data
+    """
+    #TODO: Handle a JTAG chain with more than one device
+
+    def __init__(self, tck, tms, tdi, tdo, trst_n=None, clk_period=1000):
+        self.tck = tck
+        self.clkgen = JTAG_Clock(tck, clk_period)
+        tck <= 0
+        self.tms = tms
+        tms <= 1
+        self.tdi = tdi
+        tdi <= 0
+        self.tdo = tdo
+        self.trst_n = trst_n
+        if trst_n is not None:
+            trst_n <= 1
+        self.period = Timer(clk_period)
+
+        # Standard commands
+        # TODO: make IR length configurable; now 2 is assumed
+        self.BYPASS = [1, 1]
+        self.IDCODE = [0, 1]
+        self.SAMPLEPRELOAD = [1, 0]
+        self.EXTEST = [0, 0]
+
+        # After command we always leave the controller in reset or runidle state
+        # If value is None we will always reset the interface
+        self.state = None
+
+        # The methods of this class are coroutines. The results will be stored
+        # in the result field
+        self.result = None
+
+    @cocotb.coroutine
+    def cycle_clock(self, cycles=1):
+        if self.state == "Run" and self.tms == 1:
+            self.state = "Scan"
+        yield self.clkgen.Cycle(cycles)
+
+    @cocotb.coroutine
+    def reset(self):
+        if not self.trst_n is None:
+            # Enable reset signal for one clock period
+            self.trst_n <= 0
+            yield self.period
+            self.trst_n <= 1
+        else:
+            # 5 cycles with tms on 1 should reset the JTAG TAP controller
+            self.tms <= 1
+            yield self.cycle_clock(5)
+
+        self.state = "Reset"
+
+        self.result = None
+
+    @cocotb.coroutine
+    def change_state(self, tms_list):
+        """
+        Put TAP in other state by giving a TMS sequence
+        This function does not detect if one ends up in reset or run
+        state afterwards, self.state has to be updated by caller
+        if that is the case.
+        """
+        tms_copy = list(tms_list)
+        while tms_copy:
+            self.tms <= tms_copy.pop()
+            yield self.cycle_clock()
+        self.result = None
+
+    @cocotb.coroutine
+    def change_to_run(self):
+        """
+        Put TAP in RunTestIdle state
+        self.result is bool and true if TAP went through reset state
+        """
+        isreset = False
+        if self.state is None:
+            yield self.reset()
+        if self.state is "Reset":
+            isreset = True
+            self.tms <= 0
+            yield self.cycle_clock()
+            self.state = "Run"
+        assert(self.state == "Run")
+        self.result = isreset
+
+    @cocotb.coroutine
+    def load_ir(self, cmd):
+        cmd_copy = list(cmd)
+        result = BinaryValue(bits=len(cmd_copy))
+        l_result = list()
+
+        yield self.change_to_run()
+        # Go to Capture/IR
+        yield self.change_state([0, 1, 1])
+
+        # Shift the two
+        self.tms <= 0
+        while cmd_copy:
+            # In first iteration we enter SHIFT state and tdo is made active
+            yield self.cycle_clock()
+            # For the last iteration tdi will be shifted when entering next state
+            self.tdi <= cmd_copy.pop()
+            l_result.insert(0, str(self.tdo))
+
+        # Go to RunTestIdle
+        yield self.change_state([0, 1, 1])
+        self.state = "Run"
+
+    @cocotb.coroutine
+    def idcode(self):
+        """
+        Get the IDCODE from the device
+        result will contain the 32 bit IDCODE of the device
+        """
+
+        result = BinaryValue(bits=32)
+        l_result = list()
+
+        # Keep tdi 0 for the whole run
+        self.tdi <= 0
+
+        yield self.change_to_run()
+        if not self.result:
+            # If TAP was not reset we have to load IDCODE command
+            yield self.load_ir(self.IDCODE)
+
+        # Should be again in RUN state
+        assert(self.state == "Run")
+
+        # Go to Shift/DR
+        yield self.change_state([0, 0, 1])
+
+        # Enter Shift; run for 32 cycles
+        self.tms <= 0
+        for i in range(32):
+            l_result.insert(0, str(self.tdo))
+            yield self.cycle_clock()
+        result.binstr = "".join(l_result)
+
+        # Go to RunTestIdle
+        yield self.change_state([0, 1, 1])
+        self.state = "Run"
+
+        self.result = result
+
+    @cocotb.coroutine
+    def shift_data(self, data_in):
+        """
+        Shift data in through the JTAG and capture the output
+        Input can be of type BinaryValue or an iterable value of 0 and 1s.
+        Last bit will be shifted in first.
+        result will contain the sample TDO with the same number of bits as the input
+        """
+        if isinstance(data_in, BinaryValue):
+            data_copy = [int(c) for c in data_in.binstr]
+        else:
+            data_copy = list(data_in)
+        result = BinaryValue()
+        l_result = list()
+
+        yield self.change_to_run()
+        # Go to Capture/DR
+        yield self.change_state([0, 1])
+
+        # Shift data through
+        self.tms <= 0
+        while data_copy:
+            yield self.cycle_clock()
+            self.tdi <= data_copy.pop()
+            l_result.insert(0, str(self.tdo))
+        result.binstr = "".join(l_result)
+
+        # Go to RunTestIdle
+        yield self.change_state([0, 1, 1])
+        self.state = "Run"
+
+        self.result = result
diff --git a/c4m/cocotb/jtag/c4m_jtag_svfcocotb.py b/c4m/cocotb/jtag/c4m_jtag_svfcocotb.py
new file mode 100755
index 0000000..177b7a1
--- /dev/null
+++ b/c4m/cocotb/jtag/c4m_jtag_svfcocotb.py
@@ -0,0 +1,226 @@
+import c4m_jtag_svfgrammar
+import cocotb
+from cocotb.binary import BinaryValue
+from functools import singledispatch
+
+def decodescanspec(node):
+    length = int(str(node[2]))
+    fstr = "{:0"+str(node[2])+"b}"
+
+    g_tdi = node[4]
+    g_tdo = node[5]
+    g_mask = node[6]
+    g_smask = node[7]
+
+    if g_tdi is None:
+        tdi = None
+    else:
+        tdi = BinaryValue(fstr.format(int(str(g_tdi[2]),16)), length)
+
+    if g_tdo is None:
+        tdo = None
+    else:
+        tdo = BinaryValue(fstr.format(int(str(g_tdo[3]),16)), length)
+
+    if g_mask is None:
+        mask = None
+    else:
+        mask = BinaryValue(fstr.format(int(str(g_mask[3]),16)), length)
+
+    if g_smask is None:
+        smask = None
+    else:
+        smask = BinaryValue(fstr.format(int(str(g_smask[3]),16)), length)
+
+    return (length, tdi, tdo, mask, smask)
+
+
+class SVF_Executor(object):
+    @cocotb.coroutine
+    def execute(self, node):
+        """This is the generic method"""
+        self._p("generic")
+        if False: # Make coroutine work
+            yield PythonTrigger()
+
+    @cocotb.coroutine
+    def _execute_NOP(self, node):
+        if False: # Make coroutine work
+            yield PythonTrigger()
+
+    @cocotb.coroutine
+    def _execute_EndDR(self, node):
+        self._p("EndDR ignored")
+        if False: # Make coroutine work
+            yield PythonTrigger()
+
+    @cocotb.coroutine
+    def _execute_EndIR(self, node):
+        self._p("EndIR ignored")
+        if False: # Make coroutine work
+            yield PythonTrigger()
+
+    @cocotb.coroutine
+    def _execute_Frequency(self, node):
+        self._p("Frequency ignored")
+        if False: # Make coroutine work
+            yield PythonTrigger()
+
+    @cocotb.coroutine
+    def _execute_HDR(self, node):
+        self._p("HDR ignored")
+        if False: # Make coroutine work
+            yield PythonTrigger()
+
+    @cocotb.coroutine
+    def _execute_HIR(self, node):
+        self._p("HIR ignored")
+        if False: # Make coroutine work
+            yield PythonTrigger()
+
+    @cocotb.coroutine
+    def _execute_SDR(self, node):
+        self._p("Executing SDR")
+        (length, tdi, tdo, mask, smask) = decodescanspec(node)
+
+        samelength = length == self._d_length
+        self._d_length = length
+
+        if tdi is None:
+            if not samelength:
+                raise(JTAGException("TDI needs to be specified when length of data changes"))
+        else:
+            self._d_tdi = tdi
+
+        if mask is not None:
+            self._d_mask = mask
+        elif not samelength:
+            self._d_mask = None
+
+        if smask is not None:
+            self._d_smask = smask
+        elif not samelength:
+            self._d_smask = None
+
+        yield self.master.shift_data(self._d_tdi)
+        if tdo is not None:
+            if self._d_mask is not None:
+                raise(JTAGException("MASK not supported for SDR"))
+            assert(self.result == tdo)
+
+    @cocotb.coroutine
+    def _execute_SIR(self, node):
+        (length, tdi, tdo, mask, smask) = decodescanspec(node)
+
+        samelength = length == self._i_length
+        self._i_length = length
+
+        if tdi is None:
+            if not samelength:
+                raise(JTAGException("TDI needs to be specified when length of data changes"))
+        else:
+            self._i_tdi = tdi
+
+        if mask is not None:
+            self._i_mask = mask
+        elif not samelength:
+            self._i_mask = None
+
+        if smask is not None:
+            self._i_smask = smask
+        elif not samelength:
+            self._i_smask = None
+
+        self._p("Executing SIR ({})".format(self._i_tdi.integer))
+
+        yield self.master.load_ir(self._i_tdi)
+        if tdo is not None:
+            if self._i_mask is not None:
+                raise(JTAGException("MASK not supported for SIR"))
+            assert(self.result == tdo)
+        
+
+    @cocotb.coroutine
+    def _execute_State(self, node):
+        self._p("State")
+        if False: # Make coroutine work
+            yield PythonTrigger()
+
+    @cocotb.coroutine
+    def _execute_TDR(self, node):
+        self._p("TDR")
+        if False: # Make coroutine work
+            yield PythonTrigger()
+
+    @cocotb.coroutine
+    def _execute_TIR(self, node):
+        self._p("TIR")
+        if False: # Make coroutine work
+            yield PythonTrigger()
+
+    @cocotb.coroutine
+    def _execute_Trst(self, node):
+        self._p("TRST ignored")
+        if False: # Make coroutine work
+            yield PythonTrigger()
+
+    @cocotb.coroutine
+    def _execute_Runtest(self, node):
+        if node[1] is not None:
+            raise(JTAGException("State specification for RUNTEST not supported"))
+        # TODO: cycle the right number of clocks or wait the right time
+        yield(self.master.change_state([0]))
+
+    @cocotb.coroutine
+    def _execute_SVFFile(self, node):
+        self._p("Executing SVFFile")
+        for statement in node.elements[0]:
+            yield self.execute(statement)
+
+    def __init__(self, master):
+        # master is assumed to be a JTAG_Master class
+        # it needs to support methods load_ir() and shift_data()
+        self.master = master
+
+        # Due to bug in Grammar definition all possible classes have to have
+        # a dispatch entry otherwise an error will be raised.
+        self.execute = singledispatch(self.execute)
+        self.execute.register(c4m_jtag_svfgrammar.EmptyLine, self._execute_NOP)
+        self.execute.register(c4m_jtag_svfgrammar.Comment, self._execute_NOP)
+        self.execute.register(c4m_jtag_svfgrammar.EndDR, self._execute_EndDR)
+        self.execute.register(c4m_jtag_svfgrammar.EndIR, self._execute_EndIR)
+        self.execute.register(c4m_jtag_svfgrammar.Frequency, self._execute_Frequency)
+        self.execute.register(c4m_jtag_svfgrammar.HDR, self._execute_HDR)
+        self.execute.register(c4m_jtag_svfgrammar.HIR, self._execute_HIR)
+        self.execute.register(c4m_jtag_svfgrammar.Runtest, self._execute_Runtest)
+        self.execute.register(c4m_jtag_svfgrammar.SDR, self._execute_SDR)
+        self.execute.register(c4m_jtag_svfgrammar.SIR, self._execute_SIR)
+        self.execute.register(c4m_jtag_svfgrammar.State, self._execute_State)
+        self.execute.register(c4m_jtag_svfgrammar.TDR, self._execute_TDR)
+        self.execute.register(c4m_jtag_svfgrammar.TIR, self._execute_TIR)
+        self.execute.register(c4m_jtag_svfgrammar.Trst, self._execute_Trst)
+        self.execute.register(c4m_jtag_svfgrammar.SVFFile, self._execute_SVFFile)
+
+        # Store the head and tail for the scan
+        self._d_tdi = self._d_tdi_h = self._d_tdi_t = None
+        self._d_tdo_h = self._d_tdo_t = None
+        self._i_tdi = self._i_tdi_h = self._i_tdi_t = None
+        self._i_tdo_h = self._i_tdo_t = None
+
+        # Remember the masks; smasks are ignored and bits always considered as care, e.g right
+        # value applied
+        self._d_length = self._d_length_h = self._d_length_t = None
+        self._d_mask = self._d_mask_h = self._d_mask_t = None
+        self._d_smask = self._d_smask_h = self._d_smask_t = None
+        self._i_length = self._i_length_h = self._i_length_t = None
+        self._i_mask = self._i_mask_h = self._i_mask_t = None
+        self._i_smask = self._i_smask_h = self._i_smask_t = None
+
+    @cocotb.coroutine
+    def run(self, cmds, p=print):
+        self._p = p
+        if isinstance(cmds, c4m_jtag_svfgrammar.SVFFile):
+            yield self.execute(cmds)
+        else:
+            p = c4m_jtag_svfgrammar.SVFFile.parser()
+            yield self.execute(p.parse_string(cmds))
diff --git a/c4m/cocotb/jtag/c4m_jtag_svfgrammar.py b/c4m/cocotb/jtag/c4m_jtag_svfgrammar.py
new file mode 100644
index 0000000..faf3aeb
--- /dev/null
+++ b/c4m/cocotb/jtag/c4m_jtag_svfgrammar.py
@@ -0,0 +1,96 @@
+from modgrammar import *
+
+grammar_whitespace_mode = 'explicit'
+grammar_whitespace = WS_NOEOL
+
+class SVFEol(Grammar):
+    grammar = (OPTIONAL(SPACE), EOL)
+    grammar_collapse = True
+
+class SemicolonEol(Grammar):
+    grammar = (OPTIONAL(SPACE), L(";"), SVFEol)
+    grammar_collapse = True
+
+class Integer(Grammar):
+    grammar = WORD("0-9")
+    grammar_collapse = True
+
+class Float(Grammar):
+    grammar = (Integer, (L("."), OPTIONAL(Integer)), OPTIONAL(L("E"), Integer))
+
+class Hexadecimal(Grammar):
+    grammar = (L("("), WORD("0-9A-F"), L(")"))
+    grammar_collapse = True
+
+class StableState(Grammar):
+    grammar = (L("IRPAUSE") | L("DRPAUSE") | L("RESET") | L("IDLE"))
+    grammar_collapse = True
+
+
+class ScanSpec(Grammar):
+    """The specification of Scan In/Scan out data"""
+    grammar = (SPACE, Integer, SPACE,
+               OPTIONAL(L("TDI"), OPTIONAL(SPACE), Hexadecimal),
+               OPTIONAL(OPTIONAL(SPACE), L("TDO"), OPTIONAL(SPACE), Hexadecimal),
+               OPTIONAL(OPTIONAL(SPACE), L("MASK"), OPTIONAL(SPACE), Hexadecimal),
+               OPTIONAL(OPTIONAL(SPACE), L("SMASK"), OPTIONAL(SPACE), Hexadecimal)
+              )
+    grammar_collapse = True
+
+
+class EmptyLine(Grammar):
+    grammar = SVFEol
+
+class Comment(Grammar):
+    grammar = (L("!"), REST_OF_LINE, SVFEol)
+
+class EndDR(Grammar):
+    grammar = (L("ENDDR"), SPACE, StableState, SemicolonEol)
+
+class EndIR(Grammar):
+    grammar = (L("ENDIR"), SPACE, StableState, SemicolonEol)
+
+class Frequency(Grammar):
+    grammar = (L("FREQUENCY"), OPTIONAL(SPACE, Float, OPTIONAL(SPACE), L("HZ")), OPTIONAL(SPACE), SemicolonEol)
+
+class HDR(Grammar):
+    grammar = (L("HDR"), ScanSpec, SemicolonEol)
+
+class HIR(Grammar):
+    grammar = (L("HIR"), ScanSpec, SemicolonEol)
+
+#TODO: PIO, PIOMAP
+
+class Runtest(Grammar):
+    grammar = (
+        L("RUNTEST"),
+        OPTIONAL(SPACE, StableState),
+        OPTIONAL(SPACE, Integer, OPTIONAL(SPACE), (L("TCK") | L("SCK"))),
+        OPTIONAL(SPACE, Float, OPTIONAL(SPACE), L("SEC")),
+        OPTIONAL(SPACE, L("MAXIMUM"), SPACE, Float, OPTIONAL(SPACE), L("SEC")),
+        OPTIONAL(SPACE, L("ENDSTATE"), SPACE, StableState),
+        SemicolonEol
+    )
+
+class SDR(Grammar):
+    grammar = (L("SDR"), ScanSpec, SemicolonEol)
+
+class SIR(Grammar):
+    grammar = (L("SIR"), ScanSpec, SemicolonEol)
+
+class State(Grammar):
+    # TODO: Path to reach state
+    grammar = (L("STATE"), SPACE, StableState, SemicolonEol)
+
+class TDR(Grammar):
+    grammar = (L("TDR"), ScanSpec, SemicolonEol)
+
+class TIR(Grammar):
+    grammar = (L("TIR"), ScanSpec, SemicolonEol)
+
+class Trst(Grammar):
+    grammar = (L("TRST"), SPACE, (L("ON") | L("OFF") | L("Z") | L("ABSENT")), SemicolonEol)
+
+
+class SVFFile(Grammar):
+    grammar = ONE_OR_MORE(EmptyLine | Comment | Trst | EndDR | EndIR | SIR | SDR | Runtest | State)
diff --git a/c4m/nmigen/jtag/jtag.py b/c4m/nmigen/jtag/jtag.py
new file mode 100755
index 0000000..3baaa95
--- /dev/null
+++ b/c4m/nmigen/jtag/jtag.py
@@ -0,0 +1,399 @@
+#!/bin/env python3
+import os
+
+from nmigen import *
+from nmigen.build import *
+from nmigen.lib.io import *
+
+from wishbone import Wishbone
+
+__all__ = [
+    "PmodJTAGMasterResource",
+    "PmodJTAGMasterAResource",
+    "PmodJTAGSlaveResource",
+    "PmodJTAGSlaveAResource",
+    "JTAG",
+]
+
+#TODO: Provide more documentation
+
+def PmodJTAGMasterResource(name, number, *, pmod, attrs=Attrs(IOSTANDARD="LVCMOS33")):
+    return Resource(name, number,
+        Subsignal("TCK", Pins("1", dir="o", conn=("pmod", pmod))),
+        Subsignal("TMS", Pins("2", dir="o", conn=("pmod", pmod))),
+        Subsignal("TDO", Pins("3", dir="o", conn=("pmod", pmod))),
+        Subsignal("TDI", Pins("4", dir="i", conn=("pmod", pmod))),
+        attrs,
+    )
+
+def PmodJTAGMasterAResource(name, number, *, pmod, attrs=Attrs(IOSTANDARD="LVCMOS33")):
+    return Resource(name, number,
+        Subsignal("TCK", Pins("1", dir="o", conn=("pmod", pmod))),
+        Subsignal("TMS", Pins("2", dir="o", conn=("pmod", pmod))),
+        Subsignal("TDO", Pins("3", dir="o", conn=("pmod", pmod))),
+        Subsignal("TDI", Pins("4", dir="i", conn=("pmod", pmod))),
+        Subsignal("TRST", PinsN("7", dir="o", conn=("pmod", pmod))),
+        attrs,
+    )
+
+def PmodJTAGSlaveResource(name, number, *, pmod, attrs=Attrs(IOSTANDARD="LVCMOS33")):
+    return Resource(name, number,
+        Subsignal("TCK", Pins("1", dir="i", conn=("pmod", pmod))),
+        Subsignal("TMS", Pins("2", dir="i", conn=("pmod", pmod))),
+        Subsignal("TDI", Pins("3", dir="i", conn=("pmod", pmod))),
+        Subsignal("TDO", Pins("4", dir="o", conn=("pmod", pmod))),
+        attrs,
+    )
+
+def PmodJTAGSlaveAResource(name, number, *, pmod, attrs=Attrs(IOSTANDARD="LVCMOS33")):
+    return Resource(name, number,
+        Subsignal("TCK", Pins("1", dir="i", conn=("pmod", pmod))),
+        Subsignal("TMS", Pins("2", dir="i", conn=("pmod", pmod))),
+        Subsignal("TDI", Pins("3", dir="i", conn=("pmod", pmod))),
+        Subsignal("TDO", Pins("4", dir="o", conn=("pmod", pmod))),
+        Subsignal("TRST", PinsN("7", dir="i", conn=("pmod", pmod))),
+        attrs,
+    )
+
+
+class ShiftReg(Elaboratable):
+    def __init__(self, ircodes, length, domain):
+        # The sr record will be returned to user code
+        self.sr = Record([("i", length), ("o", length), ("oe", len(ircodes)), ("ack", 1)])
+        # The next attributes are for JTAG class usage only
+        self.ir = None # made None as width is not known yet
+        self.tdi = Signal()
+        self.tdo = Signal()
+        self.tdo_en = Signal()
+        self.capture = Signal()
+        self.shift = Signal()
+        self.update = Signal()
+        self.jtag_cd = None # The JTAG clock domain
+
+        ##
+
+        self._ircodes = ircodes
+        self._domain = domain
+
+    def elaborate(self, platform):
+        length = len(self.sr.o)
+        domain = self._domain
+
+        m = Module()
+
+        m.domains.jtag = self.jtag_cd
+
+        sr_jtag = Signal(length)
+
+        assert isinstance(self.ir, Signal)
+        isir = Signal(len(self._ircodes))
+        capture = Signal()
+        shift = Signal()
+        update = Signal()
+        m.d.comb += [
+            isir.eq(Cat(self.ir == ircode for ircode in self._ircodes)),
+            capture.eq((isir != 0) & self.capture),
+            shift.eq((isir != 0) & self.shift),
+            update.eq((isir != 0) & self.update),
+        ]
+
+        # On update set o, oe and wait for ack
+        # update signal is on JTAG clockdomain, latch it
+        update_core = Signal()
+        m.d[domain] += update_core.eq(update) # This is CDC from JTAG domain to given domain
+        with m.FSM(domain=domain):
+            with m.State("IDLE"):
+                m.d.comb += self.sr.oe.eq(0)
+                with m.If(update_core):
+                    # Latch sr_jtag cross domain but it should be stable due to latching of update_core
+                    m.d[domain] += self.sr.o.eq(sr_jtag)
+                    # Wait one cycle to raise oe so sr.o has one more cycle to stabilize
+                    m.next = "WAIT4ACK"
+            with m.State("WAIT4ACK"):
+                m.d.comb += self.sr.oe.eq(isir)
+                with m.If(self.sr.ack):
+                    m.next = "WAIT4END"
+            with m.State("WAIT4END"):
+                m.d.comb += self.sr.oe.eq(0)
+                with m.If(~update_core):
+                    m.next = "IDLE"
+
+        m.d.comb += [
+            self.tdo.eq(sr_jtag[0]),
+            self.tdo_en.eq(shift),
+        ]
+
+        with m.If(shift):
+            m.d.jtag += sr_jtag.eq(Cat(sr_jtag[1:], self.tdi))
+        with m.If(capture):
+            m.d.jtag += sr_jtag.eq(self.sr.i)
+
+        return m
+
+class JTAGWishbone(Elaboratable):
+    def __init__(self, sr_addr, sr_data, wb, domain):
+        self._sr_addr = sr_addr
+        self._sr_data = sr_data
+        self._wb = wb
+        self._domain = domain
+
+        # To be set by JTAG
+        self._ir = None
+
+    def elaborate(self, platform):
+        sr_addr = self._sr_addr
+        sr_data = self._sr_data
+        wb = self._wb
+        domain = self._domain
+        ir = self._ir
+
+        m = Module()
+
+        if hasattr(wb, "sel"):
+            # Always selected
+            m.d.comb += [s.eq(1) for s in wb.sel]
+
+        # Immediately ack oe
+        m.d[domain] += [
+            sr_addr.ack.eq(sr_addr.oe),
+            sr_data.ack.eq(sr_data.oe != 0),
+        ]
+
+        with m.FSM(domain=domain) as fsm:
+            with m.State("IDLE"):
+                m.d.comb += [
+                    wb.cyc.eq(0),
+                    wb.stb.eq(0),
+                    wb.we.eq(0),
+                ]
+                with m.If(sr_addr.oe): # WBADDR code
+                    m.d[domain] += wb.addr.eq(sr_addr.o)
+                    m.next = "READ"
+                with m.If(sr_data.oe[0]): # WBREAD code
+                    m.d[domain] += wb.addr.eq(wb.addr + 1)
+                    m.next = "READ"
+                with m.If(sr_data.oe[1]): # WBWRITE code
+                    m.d[domain] += wb.dat_w.eq(sr_data.o)
+                    m.next = "WRITEREAD"
+            with m.State("READ"):
+                m.d.comb += [
+                    wb.cyc.eq(1),
+                    wb.stb.eq(1),
+                    wb.we.eq(0),
+                ]
+                with m.If(~wb.stall):
+                    m.next = "READACK"
+            with m.State("READACK"):
+                m.d.comb += [
+                    wb.cyc.eq(1),
+                    wb.stb.eq(0),
+                    wb.we.eq(0),
+                ]
+                with m.If(wb.ack):
+                    m.d[domain] += sr_data.i.eq(wb.dat_r)
+                    m.next = "IDLE"
+            with m.State("WRITEREAD"):
+                m.d.comb += [
+                    wb.cyc.eq(1),
+                    wb.stb.eq(1),
+                    wb.we.eq(1),
+                ]
+                with m.If(~wb.stall):
+                    m.next = "WRITEREADACK"
+            with m.State("WRITEREADACK"):
+                m.d.comb += [
+                    wb.cyc.eq(1),
+                    wb.stb.eq(0),
+                    wb.we.eq(0),
+                ]
+                with m.If(wb.ack):
+                    m.d[domain] += wb.addr.eq(wb.addr + 1)
+                    m.next = "READ"
+
+        return m
+
+
+class JTAG(Elaboratable):
+    @staticmethod
+    def _add_files(platform, prefix):
+        d = os.path.realpath("{0}{1}{2}{1}vhdl".format(
+            os.path.dirname(__file__), os.path.sep, os.path.pardir
+        )) + os.path.sep
+        for fname in [
+            "c4m_jtag_pkg.vhdl",
+            "c4m_jtag_idblock.vhdl",
+            "c4m_jtag_iocell.vhdl",
+            "c4m_jtag_ioblock.vhdl",
+            "c4m_jtag_irblock.vhdl",
+            "c4m_jtag_tap_fsm.vhdl",
+            "c4m_jtag_tap_controller.vhdl",
+        ]:
+            f = open(d + fname, "r")
+            platform.add_file(prefix + fname, f)
+            f.close()
+
+
+    def __init__(self, io_count, *, ir_width=None, manufacturer_id=Const(0b10001111111, 11),
+                 part_number=Const(1, 16), version=Const(0, 4)
+    ):
+        assert(isinstance(io_count, int) and io_count > 0)
+        assert((ir_width is None) or (isinstance(ir_width, int) and ir_width >= 2))
+        assert(len(version) == 4)
+
+        # TODO: Handle IOs with different directions
+        self.tck  = Signal()
+        self.tms  = Signal()
+        self.tdo  = Signal()
+        self.tdi  = Signal()
+        self.core = Array(Pin(1, "io") for _ in range(io_count)) # Signals to use for core
+        self.pad  = Array(Pin(1, "io") for _ in range(io_count)) # Signals going to IO pads
+
+        self.jtag_cd = ClockDomain(name="jtag", local=True) # Own clock domain using TCK as clock signal
+
+        ##
+
+        self._io_count = io_count
+        self._ir_width = ir_width
+        self._manufacturer_id = manufacturer_id
+        self._part_number = part_number
+        self._version = version
+
+        self._ircodes = [0, 1, 2] # Already taken codes, all ones added at the end
+        self._srs = []
+
+        self._wbs = []
+
+    def elaborate(self, platform):
+        JTAG._add_files(platform, "jtag" + os.path.sep)
+
+        m = Module()
+
+        tdo_jtag = Signal()
+        reset = Signal()
+        capture = Signal()
+        shift = Signal()
+        update = Signal()
+
+
+        ir_max = max(self._ircodes) + 1 # One extra code needed with all ones
+        ir_width = len("{:b}".format(ir_max))
+        if self._ir_width is not None:
+            assert self._ir_width >= ir_width, "Specified JTAG IR width not big enough for allocated shiift registers"
+            ir_width = self._ir_width
+        ir = Signal(ir_width)
+
+        core_i = Cat(pin.i for pin in self.core)
+        core_o = Cat(pin.o for pin in self.core)
+        core_oe = Cat(pin.oe for pin in self.core)
+        pad_i = Cat(pin.i for pin in self.pad)
+        pad_o = Cat(pin.o for pin in self.pad)
+        pad_oe = Cat(pin.oe for pin in self.pad)
+
+        params = {
+            "p_IOS": self._io_count,
+            "p_IR_WIDTH": ir_width,
+            "p_MANUFACTURER": self._manufacturer_id,
+            "p_PART_NUMBER": self._part_number,
+            "p_VERSION": self._version,
+            "i_TCK": self.tck,
+            "i_TMS": self.tms,
+            "i_TDI": self.tdi,
+            "o_TDO": tdo_jtag,
+            "i_TRST_N": Const(1),
+            "o_RESET": reset,
+            "o_DRCAPTURE": capture,
+            "o_DRSHIFT": shift,
+            "o_DRUPDATE": update,
+            "o_IR": ir,
+            "o_CORE_IN": core_i,
+            "i_CORE_OUT": core_o,
+            "i_CORE_EN": core_oe,
+            "i_PAD_IN": pad_i,
+            "o_PAD_OUT": pad_o,
+            "o_PAD_EN": pad_oe,
+        }
+        m.submodules.tap = Instance("c4m_jtag_tap_controller", **params)
+
+        m.d.comb += [
+            self.jtag_cd.clk.eq(self.tck),
+            self.jtag_cd.rst.eq(reset),
+        ]
+
+        for i, sr in enumerate(self._srs):
+            m.submodules["sr{}".format(i)] = sr
+            sr.ir = ir
+            m.d.comb += [
+                sr.tdi.eq(self.tdi),
+                sr.capture.eq(capture),
+                sr.shift.eq(shift),
+                sr.update.eq(update),
+            ]
+
+        if len(self._srs) > 0:
+            first = True
+            for sr in self._srs:
+                if first:
+                    first = False
+                    with m.If(sr.tdo_en):
+                        m.d.comb += self.tdo.eq(sr.tdo)
+                else:
+                    with m.Elif(sr.tdo_en):
+                        m.d.comb += self.tdo.eq(sr.tdo)
+            with m.Else():
+                m.d.comb += self.tdo.eq(tdo_jtag)
+        else:
+            m.d.comb += self.tdo.eq(tdo_jtag)
+
+        for i, wb in enumerate(self._wbs):
+            m.submodules["wb{}".format(i)] = wb
+            wb._ir = ir
+
+        return m
+
+
+    def add_shiftreg(self, ircode, length, domain="sync"):
+        """Add a shift register to the JTAG interface
+
+        Parameters:
+        - ircode: code(s) for the IR; int or sequence of ints. In the latter case this
+          shiftreg is shared between different IR codes.
+        - length: the length of the shift register
+        - domain: the domain on which the signal will be used"""
+
+        try:
+            ir_it = iter(ircode)
+            ircodes = ircode
+        except TypeError:
+            ir_it = ircodes = (ircode,)
+        for _ircode in ir_it:
+            assert(isinstance(_ircode, int) and _ircode > 0 and _ircode not in self._ircodes)
+
+        sr = ShiftReg(ircodes, length, domain)
+        sr.jtag_cd = self.jtag_cd
+        self._ircodes.extend(ircodes)
+        self._srs.append(sr)
+
+        return sr.sr
+
+
+    def add_wishbone(self, ircodes, address_width, data_width, sel_width=None, domain="sync"):
+        """Add a wishbone interface
+
+        Parameters:
+        - ircodes: sequence of three integer for the JTAG IR codes;
+          they represent resp. WBADDR, WBREAD and WBREADWRITE. First code
+          has a shift register of length 'address_width', the two other codes
+          share a shift register of length data_width.
+        - address_width: width of the address
+        - data_width: width of the data"""
+
+        assert len(ircodes) == 3
+
+        sr_addr = self.add_shiftreg(ircodes[0], address_width, domain=domain)
+        sr_data = self.add_shiftreg(ircodes[1:], data_width, domain=domain)
+
+        wb = Wishbone(data_width=data_width, address_width=address_width, sel_width=sel_width, master=True)
+
+        self._wbs.append(JTAGWishbone(sr_addr, sr_data, wb, domain))
+
+        return wb
diff --git a/c4m/vhdl/jtag/c4m_jtag_idblock.vhdl b/c4m/vhdl/jtag/c4m_jtag_idblock.vhdl
new file mode 100644
index 0000000..303959a
--- /dev/null
+++ b/c4m/vhdl/jtag/c4m_jtag_idblock.vhdl
@@ -0,0 +1,72 @@
+-- The JTAG id and bypass handling block
+
+library ieee;
+use ieee.std_logic_1164.ALL;
+
+use work.c4m_jtag.ALL;
+
+entity c4m_jtag_idblock is
+  generic (
+    IR_WIDTH:           integer := 2;
+    
+    MANUFACTURER:       std_logic_vector(10 downto 0) := "10001111111";
+    PART_NUMBER:        std_logic_vector(15 downto 0) := "0000000000000001";
+    VERSION:            std_logic_vector(3 downto 0) := "0000"
+  );
+  port (
+    -- needed TAP signals
+    TCK:        in std_logic;
+    TDI:        in std_logic;
+    TDO:        out std_logic;
+    TDO_EN:     out std_logic := '0';
+
+    -- JTAG state
+    STATE:      in TAPSTATE_TYPE;
+    NEXT_STATE: in TAPSTATE_TYPE;
+    DRSTATE:    in std_logic;
+
+    -- The instruction
+    IR:         in std_logic_vector(IR_WIDTH-1 downto 0)
+  );
+end c4m_jtag_idblock;
+
+architecture rtl of c4m_jtag_idblock is
+  constant IDCODE:      std_logic_vector(31 downto 0) := VERSION & PART_NUMBER & MANUFACTURER & "1";
+
+  signal SR_ID:         std_logic_vector(31 downto 0);
+  signal EN_TDO:        boolean;
+  
+  constant CMD_IDCODE:  std_logic_vector(IR_WIDTH-1 downto 0) := c4m_jtag_cmd_idcode(IR_WIDTH);
+  constant CMD_BYPASS:  std_logic_vector(IR_WIDTH-1 downto 0) := c4m_jtag_cmd_bypass(IR_WIDTH);
+begin
+  process (TCK)
+  begin
+    if rising_edge(TCK) then
+      if DRSTATE = '1' then
+        case STATE is
+          when Capture =>
+            SR_ID <= IDCODE;
+
+          when Shift =>
+            if IR = CMD_IDCODE then
+              SR_ID(30 downto 0) <= SR_ID(31 downto 1);
+              SR_ID(31) <= TDI;
+            elsif IR = CMD_BYPASS then
+              SR_ID(0) <= TDI;
+            else
+              null;
+            end if;
+
+          when others =>
+            null;
+        end case;
+      end if;
+    end if;
+  end process;
+
+  EN_TDO <= STATE = Shift and DRSTATE = '1' and (IR = CMD_IDCODE or IR = CMD_BYPASS);
+  TDO <= SR_ID(0) when EN_TDO else
+         '0';
+  TDO_EN <= '1' when EN_TDO else
+            '0';
+end rtl;
diff --git a/c4m/vhdl/jtag/c4m_jtag_ioblock.vhdl b/c4m/vhdl/jtag/c4m_jtag_ioblock.vhdl
new file mode 100644
index 0000000..c70be6f
--- /dev/null
+++ b/c4m/vhdl/jtag/c4m_jtag_ioblock.vhdl
@@ -0,0 +1,91 @@
+-- The block of io cells with JTAG boundary scan support
+
+library ieee;
+use ieee.std_logic_1164.ALL;
+
+use work.c4m_jtag.ALL;
+
+entity c4m_jtag_ioblock is
+  generic (
+    IR_WIDTH:   integer := 2;
+    IOS:        integer := 1
+  );
+  port (
+    -- needed TAP signals
+    TCK:        in std_logic;
+    TDI:        in std_logic;
+    TDO:        out std_logic;
+    TDO_EN:     out std_logic := '0';
+
+    -- JTAG state
+    STATE:      in TAPSTATE_TYPE;
+    NEXT_STATE: in TAPSTATE_TYPE;
+    DRSTATE:    in std_logic;
+
+    -- The instruction
+    IR:         in std_logic_vector(IR_WIDTH-1 downto 0);
+
+    -- The I/O access ports
+    CORE_OUT:   in std_logic_vector(IOS-1 downto 0);
+    CORE_IN:    out std_logic_vector(IOS-1 downto 0);
+    CORE_EN:    in std_logic_vector(IOS-1 downto 0);
+
+    -- The pad connections
+    PAD_OUT:    out std_logic_vector(IOS-1 downto 0);
+    PAD_IN:     in std_logic_vector(IOS-1 downto 0);
+    PAD_EN:     out std_logic_vector(IOS-1 downto 0)
+  );
+end c4m_jtag_ioblock;
+
+architecture rtl of c4m_jtag_ioblock is
+  signal IOMODE:        SRIOMODE_TYPE;
+  signal SAMPLEMODE:    SRSAMPLEMODE_TYPE;
+  signal ISSAMPLECMD:   boolean;
+  
+  signal BDSR_IN:       std_logic_vector(IOS-1 downto 0);
+  signal BDSR_OUT:      std_logic_vector(IOS-1 downto 0);
+  
+  constant CMD_SAMPLEPRELOAD: std_logic_vector(IR_WIDTH-1 downto 0) := c4m_jtag_cmd_samplepreload(IR_WIDTH);
+  constant CMD_EXTEST:  std_logic_vector(IR_WIDTH-1 downto 0) := c4m_jtag_cmd_extest(IR_WIDTH);
+begin
+  -- JTAG baundary scan IO cells
+  IOGEN: for i in 0 to IOS-1 generate
+  begin
+    IOCELL: c4m_jtag_iocell
+      port map (
+        CORE_IN => CORE_IN(i),
+        CORE_OUT => CORE_OUT(i),
+        CORE_EN => CORE_EN(i),
+        PAD_IN => PAD_IN(i),
+        PAD_OUT => PAD_OUT(i),
+        PAD_EN => PAD_EN(i),
+        BDSR_IN => BDSR_IN(i),
+        BDSR_OUT => BDSR_OUT(i),
+        IOMODE => IOMODE,
+        SAMPLEMODE => SAMPLEMODE,
+        TCK => TCK
+      );
+  end generate;
+  BDSRCONN: for i in 0 to IOS-2 generate
+  begin
+    BDSR_IN(i) <= BDSR_OUT(i+1);
+  end generate;
+  BDSR_IN(IOS-1) <= TDI;
+
+  -- Set IOMODE
+  -- Currently SR_2Core or SR_Z are not used
+  IOMODE <= SR_2Pad when IR = CMD_EXTEST else
+            SR_Through;
+
+  -- Set SAMPLEMODE
+  ISSAMPLECMD <= (IR = CMD_SAMPLEPRELOAD or IR = CMD_EXTEST) and DRSTATE = '1';
+  SAMPLEMODE <= SR_Sample when ISSAMPLECMD and STATE = Capture else
+                SR_Update when ISSAMPLECMD and STATE = Update else
+                SR_Shift when ISSAMPLECMD and STATE = Shift else
+                SR_Normal;
+
+  TDO <= BDSR_OUT(0) when ISSAMPLECMD and STATE = Shift else
+         '0';
+  TDO_EN <= '1' when ISSAMPLECMD and STATE = Shift else
+            '0';
+end rtl;
diff --git a/c4m/vhdl/jtag/c4m_jtag_iocell.vhdl b/c4m/vhdl/jtag/c4m_jtag_iocell.vhdl
new file mode 100644
index 0000000..6a218a6
--- /dev/null
+++ b/c4m/vhdl/jtag/c4m_jtag_iocell.vhdl
@@ -0,0 +1,101 @@
+-- An JTAG boundary scan for bidirectional I/O
+
+library ieee;
+use ieee.std_logic_1164.ALL;
+
+use work.c4m_jtag.ALL;
+
+entity c4m_jtag_iocell is
+  generic (
+    IR_WIDTH:   integer := 2
+  );
+  port (
+    -- core connections
+    CORE_IN:    out std_logic;
+    CORE_OUT:   in std_logic;
+    CORE_EN:    in std_logic;
+
+    -- pad connections
+    PAD_IN:     in std_logic;
+    PAD_OUT:    out std_logic;
+    PAD_EN:     out std_logic;
+
+    -- BD shift register
+    BDSR_IN:    in std_logic;
+    BDSR_OUT:   out std_logic;
+
+      -- Mode of I/O cell
+    IOMODE:     in SRIOMODE_TYPE;
+    SAMPLEMODE: in SRSAMPLEMODE_TYPE;
+    TCK:        in std_logic
+  );
+end c4m_jtag_iocell;
+
+architecture rtl of c4m_jtag_iocell is
+  signal SR_IOIN: std_logic;
+  signal SR_IOOUT: std_logic;
+  signal SR_IOEN: std_logic;
+
+  signal CORE_IN_BD: std_logic;
+  signal PAD_OUT_BD: std_logic;
+  signal PAD_EN_BD: std_logic;
+begin
+  with IOMODE select
+  CORE_IN <=
+    PAD_IN when SR_Through | SR_Z,
+    PAD_IN when SR_2Pad,
+    CORE_IN_BD when SR_2Core,
+    'X' when others;
+
+  with IOMODE select
+  PAD_OUT <=
+    CORE_OUT when SR_Through,
+    PAD_OUT_BD when SR_2Pad,
+    '0' when SR_2Core | SR_Z,  
+    'X' when others;
+
+  with IOMODE select
+  PAD_EN <=
+    CORE_EN when SR_Through,
+    PAD_EN_BD when SR_2Pad,
+    '0' when SR_2Core | SR_Z,
+    'X' when others;
+
+  process (TCK)
+  begin
+    -- Sampling of inputs and shifting of boundary scan SR needs to be done on
+    -- rising edge of TCK
+    if rising_edge(TCK) then
+      case SAMPLEMODE is
+        when SR_Sample =>
+          SR_IOIN <= PAD_IN;
+          SR_IOOUT <= CORE_OUT;
+          SR_IOEN <= CORE_EN;
+
+        when SR_Shift =>
+          SR_IOIN <= BDSR_IN;
+          SR_IOOUT <= SR_IOIN;
+          SR_IOEN <= SR_IOOUT;
+
+        when others =>
+          null;
+      end case;
+    end if;
+    
+    -- Update of output from boundary scan SR needs to be done on falling edge
+    -- of TCK
+    if falling_edge(TCK) then
+      case SAMPLEMODE is
+        when SR_Update =>
+          CORE_IN_BD <= SR_IOIN;
+          PAD_OUT_BD <= SR_IOOUT;
+          PAD_EN_BD <= SR_IOEN;
+          
+        when others =>
+          null;
+      end case;
+    end if;
+  end process;
+
+  BDSR_OUT <= SR_IOEN;
+end rtl;
diff --git a/c4m/vhdl/jtag/c4m_jtag_irblock.vhdl b/c4m/vhdl/jtag/c4m_jtag_irblock.vhdl
new file mode 100644
index 0000000..8be483a
--- /dev/null
+++ b/c4m/vhdl/jtag/c4m_jtag_irblock.vhdl
@@ -0,0 +1,64 @@
+-- Handle the instruction register for the JTAG controller
+
+library ieee;
+use ieee.std_logic_1164.ALL;
+
+use work.c4m_jtag.ALL;
+
+entity c4m_jtag_irblock is
+  generic (
+    IR_WIDTH:   integer := 2
+  );
+  port (
+    -- needed TAP signals
+    TCK:        in std_logic;
+    TDI:        in std_logic;
+    TDO:        out std_logic;
+    TDO_EN:     out std_logic := '0';
+    
+    -- JTAG state
+    STATE:      in TAPSTATE_TYPE;
+    NEXT_STATE: in TAPSTATE_TYPE;
+    IRSTATE:    in std_logic;
+
+    -- instruction register
+    IR:         out std_logic_vector(IR_WIDTH-1 downto 0)
+  );
+end c4m_jtag_irblock;
+
+architecture rtl of c4m_jtag_irblock is
+  signal SHIFT_IR:      std_logic_vector(IR_WIDTH-1 downto 0);
+
+  constant CMD_IDCODE:  std_logic_vector(IR_WIDTH-1 downto 0) := c4m_jtag_cmd_idcode(IR_WIDTH);
+begin
+  process (TCK, STATE)
+  begin
+    if STATE = TestLogicReset then
+        SHIFT_IR <= (others => '0');
+        IR <= CMD_IDCODE;
+    elsif rising_edge(TCK) then
+      if IRSTATE = '1' then
+        case STATE is
+          when Capture =>
+            SHIFT_IR(1) <= '0';
+            SHIFT_IR(0) <= '1';
+
+          when Shift =>
+            SHIFT_IR(IR_WIDTH-2 downto 0) <= SHIFT_IR(IR_WIDTH-1 downto 1);
+            SHIFT_IR(IR_WIDTH-1) <= TDI;
+
+          when Update =>
+            IR <= SHIFT_IR;
+
+          when others =>
+            null;
+        end case;
+      end if;
+    end if;
+  end process;
+
+  TDO <= SHIFT_IR(0) when STATE = Shift and IRSTATE = '1' else
+         '0';
+  TDO_EN <= '1' when STATE = Shift and IRSTATE = '1' else
+            '0';
+end rtl;
diff --git a/c4m/vhdl/jtag/c4m_jtag_pkg.vhdl b/c4m/vhdl/jtag/c4m_jtag_pkg.vhdl
new file mode 100644
index 0000000..0fc0749
--- /dev/null
+++ b/c4m/vhdl/jtag/c4m_jtag_pkg.vhdl
@@ -0,0 +1,225 @@
+-- Package of jtag support code from the Chips4Makers project
+library ieee;
+use ieee.std_logic_1164.ALL;
+
+package c4m_jtag is
+  type TAPSTATE_TYPE is (
+    TestLogicReset,
+    RunTestIdle,
+    SelectDRScan,
+    SelectIRScan,
+    Capture,
+    Shift,
+    Exit1,
+    Pause,
+    Exit2,
+    Update
+  );
+  type SRIOMODE_TYPE is (
+    SR_Through, -- Connect core signal to pad signals
+    SR_2Pad,    -- Connect BD to pad
+    SR_2Core,   -- Connect BD to core
+    SR_Z        -- pad is high impedance
+  );
+  type SRSAMPLEMODE_TYPE is (
+    SR_Normal,  -- No sampling or shifting
+    SR_Sample,  -- Sample IO state in BD SR on rising edge of TCK
+    SR_Update,  -- Update BD from SR on falling edge of TCK
+    SR_Shift    -- Shift the BD SR
+  );
+
+  component c4m_jtag_tap_fsm is
+    port (
+      -- The TAP signals
+      TCK:      in std_logic;
+      TMS:      in std_logic;
+      TRST_N:   in std_logic;
+
+      -- The state outputs
+      STATE:    out TAPSTATE_TYPE;
+      NEXT_STATE: out TAPSTATE_TYPE;
+      DRSTATE:  out std_logic;
+      IRSTATE:  out std_logic
+    );
+  end component c4m_jtag_tap_fsm;
+
+  component c4m_jtag_irblock is
+    generic (
+      IR_WIDTH: integer := 2
+    );
+    port (
+      -- needed TAP signals
+      TCK:      in std_logic;
+      TDI:      in std_logic;
+      TDO:      out std_logic;
+      TDO_EN:   out std_logic;
+    
+      -- JTAG state
+      STATE:    in TAPSTATE_TYPE;
+      NEXT_STATE: in TAPSTATE_TYPE;
+      IRSTATE:  in std_logic;
+
+      -- instruction register
+      IR:       out std_logic_vector(IR_WIDTH-1 downto 0)
+    );
+  end component c4m_jtag_irblock;
+  
+  component c4m_jtag_idblock is
+    generic (
+      IR_WIDTH:         integer := 2;
+    
+      -- The default MANUFACTURING ID is not representing a valid
+      -- manufacturer according to the JTAG standard
+      MANUFACTURER:     std_logic_vector(10 downto 0) := "10001111111";
+      PART_NUMBER:      std_logic_vector(15 downto 0) := "0000000000000001";
+      VERSION:          std_logic_vector(3 downto 0) := "0000"
+    );
+    port (
+      -- needed TAP signals
+      TCK:      in std_logic;
+      TDI:      in std_logic;
+      TDO:      out std_logic;
+      TDO_EN:   out std_logic;
+
+      -- JTAG state
+      STATE:    in TAPSTATE_TYPE;
+      NEXT_STATE: in TAPSTATE_TYPE;
+      DRSTATE:  in std_logic;
+
+      -- The instruction
+      IR:       in std_logic_vector(IR_WIDTH-1 downto 0)
+    );
+  end component c4m_jtag_idblock;
+
+  component c4m_jtag_iocell is
+    port (
+      -- core connections
+      CORE_IN:  out std_logic;
+      CORE_OUT: in std_logic;
+      CORE_EN:  in std_logic;
+
+      -- pad connections
+      PAD_IN:   in std_logic;
+      PAD_OUT:  out std_logic;
+      PAD_EN:   out std_logic;
+
+      -- BD shift register
+      BDSR_IN:  in std_logic;
+      BDSR_OUT: out std_logic;
+
+      -- Mode of I/O cell
+      IOMODE:   in SRIOMODE_TYPE;
+      SAMPLEMODE: in SRSAMPLEMODE_TYPE;
+      TCK:      in std_logic
+    );
+  end component c4m_jtag_iocell;
+  
+  component c4m_jtag_ioblock is
+    generic (
+      IR_WIDTH: integer := 2;
+      IOS:      integer := 1
+    );
+    port (
+      -- needed TAP signals
+      TCK:      in std_logic;
+      TDI:      in std_logic;
+      TDO:      out std_logic;
+      TDO_EN:   out std_logic;
+
+      -- JTAG state
+      STATE:    in TAPSTATE_TYPE;
+      NEXT_STATE: in TAPSTATE_TYPE;
+      DRSTATE:  in std_logic;
+
+      -- The instruction
+      IR:       in std_logic_vector(IR_WIDTH-1 downto 0);
+
+      -- The I/O access ports
+      CORE_OUT: in std_logic_vector(IOS-1 downto 0);
+      CORE_IN:  out std_logic_vector(IOS-1 downto 0);
+      CORE_EN:  in std_logic_vector(IOS-1 downto 0);
+
+      -- The pad connections
+      PAD_OUT:  out std_logic_vector(IOS-1 downto 0);
+      PAD_IN:   in std_logic_vector(IOS-1 downto 0);
+      PAD_EN:   out std_logic_vector(IOS-1 downto 0)
+    );
+  end component c4m_jtag_ioblock;
+
+  component c4m_jtag_tap_controller is
+    generic (
+      DEBUG:            boolean := false;
+
+      IR_WIDTH:         integer := 2;
+      IOS:              integer := 1;
+
+      -- The default MANUFACTURING ID is not representing a valid
+      -- manufacturer according to the JTAG standard
+      MANUFACTURER:     std_logic_vector(10 downto 0) := "10001111111";
+      PART_NUMBER:      std_logic_vector(15 downto 0) := "0000000000000001";
+      VERSION:          std_logic_vector(3 downto 0) := "0000"
+    );
+    port (
+      -- The TAP signals
+      TCK:      in std_logic;
+      TMS:      in std_logic;
+      TDI:      in std_logic;
+      TDO:      out std_logic;
+      TRST_N:   in std_logic;
+
+      -- The FSM state indicators
+      RESET:    out std_logic; -- In reset state
+      DRCAPTURE: out std_logic; -- In DR_Capture state
+      DRSHIFT:  out std_logic; -- In DR_Shift state
+      DRUPDATE: out std_logic; -- In DR_Update state
+
+      -- The Instruction Register
+      IR:       out std_logic_vector(IR_WIDTH-1 downto 0);
+
+      -- The I/O access ports
+      CORE_IN:  out std_logic_vector(IOS-1 downto 0);
+      CORE_EN:  in std_logic_vector(IOS-1 downto 0);
+      CORE_OUT: in std_logic_vector(IOS-1 downto 0);
+
+      -- The pad connections
+      PAD_IN:   in std_logic_vector(IOS-1 downto 0);
+      PAD_EN:   out std_logic_vector(IOS-1 downto 0);
+      PAD_OUT:  out std_logic_vector(IOS-1 downto 0)
+    );
+  end component c4m_jtag_tap_controller;
+
+  function c4m_jtag_cmd_idcode(width: integer) return std_logic_vector;
+  function c4m_jtag_cmd_bypass(width: integer) return std_logic_vector;
+  function c4m_jtag_cmd_samplepreload(width: integer) return std_logic_vector;
+  function c4m_jtag_cmd_extest(width: integer) return std_logic_vector;
+end c4m_jtag;
+
+package body c4m_jtag is
+  function c4m_jtag_cmd_bypass(width: integer) return std_logic_vector is
+    variable return_vector: std_logic_vector(width-1 downto 0);
+  begin
+    return_vector := (others => '1');
+    return return_vector;
+  end;
+
+  function c4m_jtag_cmd_idcode(width: integer) return std_logic_vector is
+    variable return_vector: std_logic_vector(width-1 downto 0);
+  begin
+    return_vector := (0 => '1', others => '0');
+    return return_vector;
+  end;
+
+  function c4m_jtag_cmd_samplepreload(width: integer) return std_logic_vector is
+    variable return_vector: std_logic_vector(width-1 downto 0);
+  begin
+    return_vector := (1 => '1', others => '0');
+    return return_vector;
+  end;
+
+  function c4m_jtag_cmd_extest(width: integer) return std_logic_vector is
+    variable return_vector: std_logic_vector(width-1 downto 0);
+  begin
+    return_vector := (others => '0');
+    return return_vector;
+  end;
+end package body;
diff --git a/c4m/vhdl/jtag/c4m_jtag_tap_controller.vhdl b/c4m/vhdl/jtag/c4m_jtag_tap_controller.vhdl
new file mode 100644
index 0000000..217982e
--- /dev/null
+++ b/c4m/vhdl/jtag/c4m_jtag_tap_controller.vhdl
@@ -0,0 +1,154 @@
+-- A JTAG complient tap controller implementation
+-- This is implemented based on the IEEE 1149.1 standard
+
+library ieee;
+use ieee.std_logic_1164.ALL;
+
+use work.c4m_jtag.ALL;
+
+entity c4m_jtag_tap_controller is
+  generic (
+    DEBUG:              boolean := false;
+
+    IR_WIDTH:           integer := 2;
+    IOS:                integer := 1;
+
+    MANUFACTURER:       std_logic_vector(10 downto 0) := "10001111111";
+    PART_NUMBER:        std_logic_vector(15 downto 0) := "0000000000000001";
+    VERSION:            std_logic_vector(3 downto 0) := "0000"
+  );
+  port (
+    -- The TAP signals
+    TCK:        in std_logic;
+    TMS:        in std_logic;
+    TDI:        in std_logic;
+    TDO:        out std_logic;
+    TRST_N:     in std_logic;
+
+    -- The FSM state indicators
+    RESET:      out std_logic;
+    DRCAPTURE:  out std_logic;
+    DRSHIFT:    out std_logic;
+    DRUPDATE:   out std_logic;
+
+    -- The Instruction Register
+    IR:         out std_logic_vector(IR_WIDTH-1 downto 0);
+
+    -- The I/O access ports
+    CORE_IN:    out std_logic_vector(IOS-1 downto 0);
+    CORE_EN:    in std_logic_vector(IOS-1 downto 0);
+    CORE_OUT:   in std_logic_vector(IOS-1 downto 0);
+
+    -- The pad connections
+    PAD_IN:     in std_logic_vector(IOS-1 downto 0);
+    PAD_EN:     out std_logic_vector(IOS-1 downto 0);
+    PAD_OUT:    out std_logic_vector(IOS-1 downto 0)
+  );
+end c4m_jtag_tap_controller;
+
+architecture rtl of c4m_jtag_tap_controller is
+  signal S_STATE:       TAPSTATE_TYPE;
+  signal S_NEXT_STATE:  TAPSTATE_TYPE;
+  signal S_IRSTATE:     std_logic;
+  signal S_DRSTATE:     std_logic;
+  signal S_IR:          std_logic_vector(IR_WIDTH-1 downto 0);
+
+  signal IR_TDO:        std_logic;
+  signal IR_TDO_EN:     std_logic;
+  signal ID_TDO:        std_logic;
+  signal ID_TDO_EN:     std_logic;
+  signal IO_TDO:        std_logic;
+  signal IO_TDO_EN:     std_logic;
+begin
+  IR <= S_IR;
+
+  RESET     <= '1' when S_STATE = TestLogicReset              else '0';
+  DRCAPTURE <= '1' when S_STATE = Capture and S_DRSTATE = '1' else '0';
+  DRSHIFT   <= '1' when S_STATE = Shift   and S_DRSTATE = '1' else '0';
+  DRUPDATE  <= '1' when S_STATE = Update  and S_DRSTATE = '1' else '0';
+
+  -- JTAG state machine
+  FSM:  c4m_jtag_tap_fsm
+    port map (
+      TCK => TCK,
+      TMS => TMS,
+      TRST_N => TRST_N,
+      STATE => S_STATE,
+      NEXT_STATE => S_NEXT_STATE,
+      DRSTATE => S_DRSTATE,
+      IRSTATE => S_IRSTATE
+    );
+
+  -- The instruction register
+  IRBLOCK: c4m_jtag_irblock
+    generic map (
+      IR_WIDTH => IR_WIDTH
+    )
+    port map (
+      TCK => TCK,
+      TDI => TDI,
+      TDO => IR_TDO,
+      TDO_EN => IR_TDO_EN,
+      STATE => S_STATE,
+      NEXT_STATE => S_NEXT_STATE,
+      IRSTATE => S_IRSTATE,
+      IR => S_IR
+    );
+
+  -- The ID
+  IDBLOCK: c4m_jtag_idblock
+    generic map (
+      IR_WIDTH => IR_WIDTH,
+      PART_NUMBER => PART_NUMBER,
+      MANUFACTURER => MANUFACTURER
+    )
+    port map (
+      TCK => TCK,
+      TDI => TDI,
+      TDO => ID_TDO,
+      TDO_EN => ID_TDO_EN,
+      STATE => S_STATE,
+      NEXT_STATE => S_NEXT_STATE,
+      DRSTATE => S_DRSTATE,
+      IR => S_IR
+    );
+  
+  -- The IOS
+  IOBLOCK: c4m_jtag_ioblock
+    generic map (
+      IR_WIDTH => IR_WIDTH,
+      IOS => IOS
+    )
+    port map (
+      TCK => TCK,
+      TDI => TDI,
+      TDO => IO_TDO,
+      TDO_EN => IO_TDO_EN,
+      STATE => S_STATE,
+      NEXT_STATE => S_NEXT_STATE,
+      DRSTATE => S_DRSTATE,
+      IR => S_IR,
+      CORE_OUT => CORE_OUT,
+      CORE_IN => CORE_IN,
+      CORE_EN => CORE_EN,
+      PAD_OUT => PAD_OUT,
+      PAD_IN => PAD_IN,
+      PAD_EN => PAD_EN
+    );
+
+  TDO <= IR_TDO when IR_TDO_EN = '1' else
+         ID_TDO when ID_TDO_EN = '1' else
+         IO_TDO when IO_TDO_EN = '1' else
+         '0';
+
+  CHECK_EN: if DEBUG generate
+    signal EN:  std_logic_vector(2 downto 0) := "000";
+  begin
+    EN <= IR_TDO_EN & ID_TDO_EN & IO_TDO_EN;
+    assert EN = "000" or EN = "100" or EN = "010" or EN = "001"
+      report "TDO conflict in c4m_jtag_tap_controller"
+      severity ERROR;
+  end generate CHECK_EN;
+end rtl;
+
+
diff --git a/c4m/vhdl/jtag/c4m_jtag_tap_fsm.vhdl b/c4m/vhdl/jtag/c4m_jtag_tap_fsm.vhdl
new file mode 100644
index 0000000..005eccb
--- /dev/null
+++ b/c4m/vhdl/jtag/c4m_jtag_tap_fsm.vhdl
@@ -0,0 +1,140 @@
+-- The JTAG state machine
+-- This is implemented based on the IEEE 1149.1 standard
+
+library ieee;
+use ieee.std_logic_1164.ALL;
+
+use work.c4m_jtag.ALL;
+
+entity c4m_jtag_tap_fsm is
+  port (
+    -- The TAP signals
+    TCK:        in std_logic;
+    TMS:        in std_logic;
+    TRST_N:     in std_logic;
+
+    -- The state outputs
+    STATE:      out TAPSTATE_TYPE;
+    NEXT_STATE: out TAPSTATE_TYPE;
+    DRSTATE:    out std_logic;
+    IRSTATE:    out std_logic
+  );
+end c4m_jtag_tap_fsm;
+
+architecture rtl of c4m_jtag_tap_fsm is
+  signal S_STATE:         TAPSTATE_TYPE;
+  signal S_NEXT_STATE:    TAPSTATE_TYPE;
+  signal S_DRSTATE:       std_logic;
+  signal S_IRSTATE:       std_logic;
+  signal NEXT_DRSTATE:    std_logic;
+  signal NEXT_IRSTATE:    std_logic;
+begin
+  STATE <= S_STATE;
+  NEXT_STATE <= S_NEXT_STATE;
+  DRSTATE <= S_DRSTATE;
+  IRSTATE <= S_IRSTATE;
+  
+  process (TCK, TRST_N)
+  begin
+    if TRST_N = '0' then
+      S_DRSTATE <= '0';
+      S_IRSTATE <= '0';
+      S_STATE <= TestLogicReset;
+    elsif rising_edge(TCK) then
+      S_STATE <= S_NEXT_STATE;
+      S_DRSTATE <= NEXT_DRSTATE;
+      S_IRSTATE <= NEXT_IRSTATE;
+    end if;
+  end process;
+
+  NEXT_DRSTATE <=
+    '0' when S_NEXT_STATE = TestLogicReset else
+    '0' when S_NEXT_STATE = RunTestIdle else
+    '1' when S_NEXT_STATE = SelectDRScan else
+    '0' when S_NEXT_STATE = SelectIRScan else
+    S_DRSTATE;
+  NEXT_IRSTATE <=
+    '0' when S_NEXT_STATE = TestLogicReset else
+    '0' when S_NEXT_STATE = RunTestIdle else
+    '0' when S_NEXT_STATE = SelectDRScan else
+    '1' when S_NEXT_STATE = SelectIRScan else
+    S_IRSTATE;
+
+  process (S_STATE, TMS)
+  begin
+    case S_STATE is
+      when TestLogicReset =>
+        if (TMS = '0') then
+          S_NEXT_STATE <= RunTestIdle;
+        else
+          S_NEXT_STATE <= TestLogicReset;
+        end if;
+
+      when RunTestIdle =>
+        if (TMS = '0') then
+          S_NEXT_STATE <= RunTestIdle;
+        else
+          S_NEXT_STATE <= SelectDRScan;
+        end if;
+
+      when SelectDRScan =>
+        if (TMS = '0') then
+          S_NEXT_STATE <= Capture;
+        else
+          S_NEXT_STATE <= SelectIRScan;
+        end if;
+
+      when SelectIRScan =>
+        if (TMS = '0') then
+          S_NEXT_STATE <= Capture;
+        else
+          S_NEXT_STATE <= TestLogicReset;
+        end if;
+
+      when Capture =>
+        if (TMS = '0') then
+          S_NEXT_STATE <= Shift;
+        else
+          S_NEXT_STATE <= Exit1;
+        end if;
+
+      when Shift =>
+        if (TMS = '0') then
+          S_NEXT_STATE <= Shift;
+        else
+          S_NEXT_STATE <= Exit1;
+        end if;
+
+      when Exit1 =>
+        if (TMS = '0') then
+          S_NEXT_STATE <= Pause;
+        else
+          S_NEXT_STATE <= Update;
+        end if;
+
+      when Pause =>
+        if (TMS = '0') then
+          S_NEXT_STATE <= Pause;
+        else
+          S_NEXT_STATE <= Exit2;
+        end if;
+
+      when Exit2 =>
+        if (TMS = '0') then
+          S_NEXT_STATE <= Shift;
+        else
+          S_NEXT_STATE <= Update;
+        end if;
+
+      when Update =>
+        if (TMS = '0') then
+          S_NEXT_STATE <= RunTestIdle;
+        else
+          S_NEXT_STATE <= SelectDRScan;
+        end if;
+
+      when others =>
+        S_NEXT_STATE <= TestLogicReset;
+    end case;
+  end process;
+end rtl;
diff --git a/rtl/nmigen/jtag.py b/rtl/nmigen/jtag.py
deleted file mode 100755
index 3baaa95..0000000
--- a/rtl/nmigen/jtag.py
+++ /dev/null
@@ -1,399 +0,0 @@
-#!/bin/env python3
-import os
-
-from nmigen import *
-from nmigen.build import *
-from nmigen.lib.io import *
-
-from wishbone import Wishbone
-
-__all__ = [
-    "PmodJTAGMasterResource",
-    "PmodJTAGMasterAResource",
-    "PmodJTAGSlaveResource",
-    "PmodJTAGSlaveAResource",
-    "JTAG",
-]
-
-#TODO: Provide more documentation
-
-def PmodJTAGMasterResource(name, number, *, pmod, attrs=Attrs(IOSTANDARD="LVCMOS33")):
-    return Resource(name, number,
-        Subsignal("TCK", Pins("1", dir="o", conn=("pmod", pmod))),
-        Subsignal("TMS", Pins("2", dir="o", conn=("pmod", pmod))),
-        Subsignal("TDO", Pins("3", dir="o", conn=("pmod", pmod))),
-        Subsignal("TDI", Pins("4", dir="i", conn=("pmod", pmod))),
-        attrs,
-    )
-
-def PmodJTAGMasterAResource(name, number, *, pmod, attrs=Attrs(IOSTANDARD="LVCMOS33")):
-    return Resource(name, number,
-        Subsignal("TCK", Pins("1", dir="o", conn=("pmod", pmod))),
-        Subsignal("TMS", Pins("2", dir="o", conn=("pmod", pmod))),
-        Subsignal("TDO", Pins("3", dir="o", conn=("pmod", pmod))),
-        Subsignal("TDI", Pins("4", dir="i", conn=("pmod", pmod))),
-        Subsignal("TRST", PinsN("7", dir="o", conn=("pmod", pmod))),
-        attrs,
-    )
-
-def PmodJTAGSlaveResource(name, number, *, pmod, attrs=Attrs(IOSTANDARD="LVCMOS33")):
-    return Resource(name, number,
-        Subsignal("TCK", Pins("1", dir="i", conn=("pmod", pmod))),
-        Subsignal("TMS", Pins("2", dir="i", conn=("pmod", pmod))),
-        Subsignal("TDI", Pins("3", dir="i", conn=("pmod", pmod))),
-        Subsignal("TDO", Pins("4", dir="o", conn=("pmod", pmod))),
-        attrs,
-    )
-
-def PmodJTAGSlaveAResource(name, number, *, pmod, attrs=Attrs(IOSTANDARD="LVCMOS33")):
-    return Resource(name, number,
-        Subsignal("TCK", Pins("1", dir="i", conn=("pmod", pmod))),
-        Subsignal("TMS", Pins("2", dir="i", conn=("pmod", pmod))),
-        Subsignal("TDI", Pins("3", dir="i", conn=("pmod", pmod))),
-        Subsignal("TDO", Pins("4", dir="o", conn=("pmod", pmod))),
-        Subsignal("TRST", PinsN("7", dir="i", conn=("pmod", pmod))),
-        attrs,
-    )
-
-
-class ShiftReg(Elaboratable):
-    def __init__(self, ircodes, length, domain):
-        # The sr record will be returned to user code
-        self.sr = Record([("i", length), ("o", length), ("oe", len(ircodes)), ("ack", 1)])
-        # The next attributes are for JTAG class usage only
-        self.ir = None # made None as width is not known yet
-        self.tdi = Signal()
-        self.tdo = Signal()
-        self.tdo_en = Signal()
-        self.capture = Signal()
-        self.shift = Signal()
-        self.update = Signal()
-        self.jtag_cd = None # The JTAG clock domain
-
-        ##
-
-        self._ircodes = ircodes
-        self._domain = domain
-
-    def elaborate(self, platform):
-        length = len(self.sr.o)
-        domain = self._domain
-
-        m = Module()
-
-        m.domains.jtag = self.jtag_cd
-
-        sr_jtag = Signal(length)
-
-        assert isinstance(self.ir, Signal)
-        isir = Signal(len(self._ircodes))
-        capture = Signal()
-        shift = Signal()
-        update = Signal()
-        m.d.comb += [
-            isir.eq(Cat(self.ir == ircode for ircode in self._ircodes)),
-            capture.eq((isir != 0) & self.capture),
-            shift.eq((isir != 0) & self.shift),
-            update.eq((isir != 0) & self.update),
-        ]
-
-        # On update set o, oe and wait for ack
-        # update signal is on JTAG clockdomain, latch it
-        update_core = Signal()
-        m.d[domain] += update_core.eq(update) # This is CDC from JTAG domain to given domain
-        with m.FSM(domain=domain):
-            with m.State("IDLE"):
-                m.d.comb += self.sr.oe.eq(0)
-                with m.If(update_core):
-                    # Latch sr_jtag cross domain but it should be stable due to latching of update_core
-                    m.d[domain] += self.sr.o.eq(sr_jtag)
-                    # Wait one cycle to raise oe so sr.o has one more cycle to stabilize
-                    m.next = "WAIT4ACK"
-            with m.State("WAIT4ACK"):
-                m.d.comb += self.sr.oe.eq(isir)
-                with m.If(self.sr.ack):
-                    m.next = "WAIT4END"
-            with m.State("WAIT4END"):
-                m.d.comb += self.sr.oe.eq(0)
-                with m.If(~update_core):
-                    m.next = "IDLE"
-
-        m.d.comb += [
-            self.tdo.eq(sr_jtag[0]),
-            self.tdo_en.eq(shift),
-        ]
-
-        with m.If(shift):
-            m.d.jtag += sr_jtag.eq(Cat(sr_jtag[1:], self.tdi))
-        with m.If(capture):
-            m.d.jtag += sr_jtag.eq(self.sr.i)
-
-        return m
-
-class JTAGWishbone(Elaboratable):
-    def __init__(self, sr_addr, sr_data, wb, domain):
-        self._sr_addr = sr_addr
-        self._sr_data = sr_data
-        self._wb = wb
-        self._domain = domain
-
-        # To be set by JTAG
-        self._ir = None
-
-    def elaborate(self, platform):
-        sr_addr = self._sr_addr
-        sr_data = self._sr_data
-        wb = self._wb
-        domain = self._domain
-        ir = self._ir
-
-        m = Module()
-
-        if hasattr(wb, "sel"):
-            # Always selected
-            m.d.comb += [s.eq(1) for s in wb.sel]
-
-        # Immediately ack oe
-        m.d[domain] += [
-            sr_addr.ack.eq(sr_addr.oe),
-            sr_data.ack.eq(sr_data.oe != 0),
-        ]
-
-        with m.FSM(domain=domain) as fsm:
-            with m.State("IDLE"):
-                m.d.comb += [
-                    wb.cyc.eq(0),
-                    wb.stb.eq(0),
-                    wb.we.eq(0),
-                ]
-                with m.If(sr_addr.oe): # WBADDR code
-                    m.d[domain] += wb.addr.eq(sr_addr.o)
-                    m.next = "READ"
-                with m.If(sr_data.oe[0]): # WBREAD code
-                    m.d[domain] += wb.addr.eq(wb.addr + 1)
-                    m.next = "READ"
-                with m.If(sr_data.oe[1]): # WBWRITE code
-                    m.d[domain] += wb.dat_w.eq(sr_data.o)
-                    m.next = "WRITEREAD"
-            with m.State("READ"):
-                m.d.comb += [
-                    wb.cyc.eq(1),
-                    wb.stb.eq(1),
-                    wb.we.eq(0),
-                ]
-                with m.If(~wb.stall):
-                    m.next = "READACK"
-            with m.State("READACK"):
-                m.d.comb += [
-                    wb.cyc.eq(1),
-                    wb.stb.eq(0),
-                    wb.we.eq(0),
-                ]
-                with m.If(wb.ack):
-                    m.d[domain] += sr_data.i.eq(wb.dat_r)
-                    m.next = "IDLE"
-            with m.State("WRITEREAD"):
-                m.d.comb += [
-                    wb.cyc.eq(1),
-                    wb.stb.eq(1),
-                    wb.we.eq(1),
-                ]
-                with m.If(~wb.stall):
-                    m.next = "WRITEREADACK"
-            with m.State("WRITEREADACK"):
-                m.d.comb += [
-                    wb.cyc.eq(1),
-                    wb.stb.eq(0),
-                    wb.we.eq(0),
-                ]
-                with m.If(wb.ack):
-                    m.d[domain] += wb.addr.eq(wb.addr + 1)
-                    m.next = "READ"
-
-        return m
-
-
-class JTAG(Elaboratable):
-    @staticmethod
-    def _add_files(platform, prefix):
-        d = os.path.realpath("{0}{1}{2}{1}vhdl".format(
-            os.path.dirname(__file__), os.path.sep, os.path.pardir
-        )) + os.path.sep
-        for fname in [
-            "c4m_jtag_pkg.vhdl",
-            "c4m_jtag_idblock.vhdl",
-            "c4m_jtag_iocell.vhdl",
-            "c4m_jtag_ioblock.vhdl",
-            "c4m_jtag_irblock.vhdl",
-            "c4m_jtag_tap_fsm.vhdl",
-            "c4m_jtag_tap_controller.vhdl",
-        ]:
-            f = open(d + fname, "r")
-            platform.add_file(prefix + fname, f)
-            f.close()
-
-
-    def __init__(self, io_count, *, ir_width=None, manufacturer_id=Const(0b10001111111, 11),
-                 part_number=Const(1, 16), version=Const(0, 4)
-    ):
-        assert(isinstance(io_count, int) and io_count > 0)
-        assert((ir_width is None) or (isinstance(ir_width, int) and ir_width >= 2))
-        assert(len(version) == 4)
-
-        # TODO: Handle IOs with different directions
-        self.tck  = Signal()
-        self.tms  = Signal()
-        self.tdo  = Signal()
-        self.tdi  = Signal()
-        self.core = Array(Pin(1, "io") for _ in range(io_count)) # Signals to use for core
-        self.pad  = Array(Pin(1, "io") for _ in range(io_count)) # Signals going to IO pads
-
-        self.jtag_cd = ClockDomain(name="jtag", local=True) # Own clock domain using TCK as clock signal
-
-        ##
-
-        self._io_count = io_count
-        self._ir_width = ir_width
-        self._manufacturer_id = manufacturer_id
-        self._part_number = part_number
-        self._version = version
-
-        self._ircodes = [0, 1, 2] # Already taken codes, all ones added at the end
-        self._srs = []
-
-        self._wbs = []
-
-    def elaborate(self, platform):
-        JTAG._add_files(platform, "jtag" + os.path.sep)
-
-        m = Module()
-
-        tdo_jtag = Signal()
-        reset = Signal()
-        capture = Signal()
-        shift = Signal()
-        update = Signal()
-
-
-        ir_max = max(self._ircodes) + 1 # One extra code needed with all ones
-        ir_width = len("{:b}".format(ir_max))
-        if self._ir_width is not None:
-            assert self._ir_width >= ir_width, "Specified JTAG IR width not big enough for allocated shiift registers"
-            ir_width = self._ir_width
-        ir = Signal(ir_width)
-
-        core_i = Cat(pin.i for pin in self.core)
-        core_o = Cat(pin.o for pin in self.core)
-        core_oe = Cat(pin.oe for pin in self.core)
-        pad_i = Cat(pin.i for pin in self.pad)
-        pad_o = Cat(pin.o for pin in self.pad)
-        pad_oe = Cat(pin.oe for pin in self.pad)
-
-        params = {
-            "p_IOS": self._io_count,
-            "p_IR_WIDTH": ir_width,
-            "p_MANUFACTURER": self._manufacturer_id,
-            "p_PART_NUMBER": self._part_number,
-            "p_VERSION": self._version,
-            "i_TCK": self.tck,
-            "i_TMS": self.tms,
-            "i_TDI": self.tdi,
-            "o_TDO": tdo_jtag,
-            "i_TRST_N": Const(1),
-            "o_RESET": reset,
-            "o_DRCAPTURE": capture,
-            "o_DRSHIFT": shift,
-            "o_DRUPDATE": update,
-            "o_IR": ir,
-            "o_CORE_IN": core_i,
-            "i_CORE_OUT": core_o,
-            "i_CORE_EN": core_oe,
-            "i_PAD_IN": pad_i,
-            "o_PAD_OUT": pad_o,
-            "o_PAD_EN": pad_oe,
-        }
-        m.submodules.tap = Instance("c4m_jtag_tap_controller", **params)
-
-        m.d.comb += [
-            self.jtag_cd.clk.eq(self.tck),
-            self.jtag_cd.rst.eq(reset),
-        ]
-
-        for i, sr in enumerate(self._srs):
-            m.submodules["sr{}".format(i)] = sr
-            sr.ir = ir
-            m.d.comb += [
-                sr.tdi.eq(self.tdi),
-                sr.capture.eq(capture),
-                sr.shift.eq(shift),
-                sr.update.eq(update),
-            ]
-
-        if len(self._srs) > 0:
-            first = True
-            for sr in self._srs:
-                if first:
-                    first = False
-                    with m.If(sr.tdo_en):
-                        m.d.comb += self.tdo.eq(sr.tdo)
-                else:
-                    with m.Elif(sr.tdo_en):
-                        m.d.comb += self.tdo.eq(sr.tdo)
-            with m.Else():
-                m.d.comb += self.tdo.eq(tdo_jtag)
-        else:
-            m.d.comb += self.tdo.eq(tdo_jtag)
-
-        for i, wb in enumerate(self._wbs):
-            m.submodules["wb{}".format(i)] = wb
-            wb._ir = ir
-
-        return m
-
-
-    def add_shiftreg(self, ircode, length, domain="sync"):
-        """Add a shift register to the JTAG interface
-
-        Parameters:
-        - ircode: code(s) for the IR; int or sequence of ints. In the latter case this
-          shiftreg is shared between different IR codes.
-        - length: the length of the shift register
-        - domain: the domain on which the signal will be used"""
-
-        try:
-            ir_it = iter(ircode)
-            ircodes = ircode
-        except TypeError:
-            ir_it = ircodes = (ircode,)
-        for _ircode in ir_it:
-            assert(isinstance(_ircode, int) and _ircode > 0 and _ircode not in self._ircodes)
-
-        sr = ShiftReg(ircodes, length, domain)
-        sr.jtag_cd = self.jtag_cd
-        self._ircodes.extend(ircodes)
-        self._srs.append(sr)
-
-        return sr.sr
-
-
-    def add_wishbone(self, ircodes, address_width, data_width, sel_width=None, domain="sync"):
-        """Add a wishbone interface
-
-        Parameters:
-        - ircodes: sequence of three integer for the JTAG IR codes;
-          they represent resp. WBADDR, WBREAD and WBREADWRITE. First code
-          has a shift register of length 'address_width', the two other codes
-          share a shift register of length data_width.
-        - address_width: width of the address
-        - data_width: width of the data"""
-
-        assert len(ircodes) == 3
-
-        sr_addr = self.add_shiftreg(ircodes[0], address_width, domain=domain)
-        sr_data = self.add_shiftreg(ircodes[1:], data_width, domain=domain)
-
-        wb = Wishbone(data_width=data_width, address_width=address_width, sel_width=sel_width, master=True)
-
-        self._wbs.append(JTAGWishbone(sr_addr, sr_data, wb, domain))
-
-        return wb
diff --git a/rtl/vhdl/c4m_jtag_idblock.vhdl b/rtl/vhdl/c4m_jtag_idblock.vhdl
deleted file mode 100644
index 303959a..0000000
--- a/rtl/vhdl/c4m_jtag_idblock.vhdl
+++ /dev/null
@@ -1,72 +0,0 @@
--- The JTAG id and bypass handling block
-
-library ieee;
-use ieee.std_logic_1164.ALL;
-
-use work.c4m_jtag.ALL;
-
-entity c4m_jtag_idblock is
-  generic (
-    IR_WIDTH:           integer := 2;
-    
-    MANUFACTURER:       std_logic_vector(10 downto 0) := "10001111111";
-    PART_NUMBER:        std_logic_vector(15 downto 0) := "0000000000000001";
-    VERSION:            std_logic_vector(3 downto 0) := "0000"
-  );
-  port (
-    -- needed TAP signals
-    TCK:        in std_logic;
-    TDI:        in std_logic;
-    TDO:        out std_logic;
-    TDO_EN:     out std_logic := '0';
-
-    -- JTAG state
-    STATE:      in TAPSTATE_TYPE;
-    NEXT_STATE: in TAPSTATE_TYPE;
-    DRSTATE:    in std_logic;
-
-    -- The instruction
-    IR:         in std_logic_vector(IR_WIDTH-1 downto 0)
-  );
-end c4m_jtag_idblock;
-
-architecture rtl of c4m_jtag_idblock is
-  constant IDCODE:      std_logic_vector(31 downto 0) := VERSION & PART_NUMBER & MANUFACTURER & "1";
-
-  signal SR_ID:         std_logic_vector(31 downto 0);
-  signal EN_TDO:        boolean;
-  
-  constant CMD_IDCODE:  std_logic_vector(IR_WIDTH-1 downto 0) := c4m_jtag_cmd_idcode(IR_WIDTH);
-  constant CMD_BYPASS:  std_logic_vector(IR_WIDTH-1 downto 0) := c4m_jtag_cmd_bypass(IR_WIDTH);
-begin
-  process (TCK)
-  begin
-    if rising_edge(TCK) then
-      if DRSTATE = '1' then
-        case STATE is
-          when Capture =>
-            SR_ID <= IDCODE;
-
-          when Shift =>
-            if IR = CMD_IDCODE then
-              SR_ID(30 downto 0) <= SR_ID(31 downto 1);
-              SR_ID(31) <= TDI;
-            elsif IR = CMD_BYPASS then
-              SR_ID(0) <= TDI;
-            else
-              null;
-            end if;
-
-          when others =>
-            null;
-        end case;
-      end if;
-    end if;
-  end process;
-
-  EN_TDO <= STATE = Shift and DRSTATE = '1' and (IR = CMD_IDCODE or IR = CMD_BYPASS);
-  TDO <= SR_ID(0) when EN_TDO else
-         '0';
-  TDO_EN <= '1' when EN_TDO else
-            '0';
-end rtl;
diff --git a/rtl/vhdl/c4m_jtag_ioblock.vhdl b/rtl/vhdl/c4m_jtag_ioblock.vhdl
deleted file mode 100644
index c70be6f..0000000
--- a/rtl/vhdl/c4m_jtag_ioblock.vhdl
+++ /dev/null
@@ -1,91 +0,0 @@
--- The block of io cells with JTAG boundary scan support
-
-library ieee;
-use ieee.std_logic_1164.ALL;
-
-use work.c4m_jtag.ALL;
-
-entity c4m_jtag_ioblock is
-  generic (
-    IR_WIDTH:   integer := 2;
-    IOS:        integer := 1
-  );
-  port (
-    -- needed TAP signals
-    TCK:        in std_logic;
-    TDI:        in std_logic;
-    TDO:        out std_logic;
-    TDO_EN:     out std_logic := '0';
-
-    -- JTAG state
-    STATE:      in TAPSTATE_TYPE;
-    NEXT_STATE: in TAPSTATE_TYPE;
-    DRSTATE:    in std_logic;
-
-    -- The instruction
-    IR:         in std_logic_vector(IR_WIDTH-1 downto 0);
-
-    -- The I/O access ports
-    CORE_OUT:   in std_logic_vector(IOS-1 downto 0);
-    CORE_IN:    out std_logic_vector(IOS-1 downto 0);
-    CORE_EN:    in std_logic_vector(IOS-1 downto 0);
-
-    -- The pad connections
-    PAD_OUT:    out std_logic_vector(IOS-1 downto 0);
-    PAD_IN:     in std_logic_vector(IOS-1 downto 0);
-    PAD_EN:     out std_logic_vector(IOS-1 downto 0)
-  );
-end c4m_jtag_ioblock;
-
-architecture rtl of c4m_jtag_ioblock is
-  signal IOMODE:        SRIOMODE_TYPE;
-  signal SAMPLEMODE:    SRSAMPLEMODE_TYPE;
-  signal ISSAMPLECMD:   boolean;
-  
-  signal BDSR_IN:       std_logic_vector(IOS-1 downto 0);
-  signal BDSR_OUT:      std_logic_vector(IOS-1 downto 0);
-  
-  constant CMD_SAMPLEPRELOAD: std_logic_vector(IR_WIDTH-1 downto 0) := c4m_jtag_cmd_samplepreload(IR_WIDTH);
-  constant CMD_EXTEST:  std_logic_vector(IR_WIDTH-1 downto 0) := c4m_jtag_cmd_extest(IR_WIDTH);
-begin
-  -- JTAG baundary scan IO cells
-  IOGEN: for i in 0 to IOS-1 generate
-  begin
-    IOCELL: c4m_jtag_iocell
-      port map (
-        CORE_IN => CORE_IN(i),
-        CORE_OUT => CORE_OUT(i),
-        CORE_EN => CORE_EN(i),
-        PAD_IN => PAD_IN(i),
-        PAD_OUT => PAD_OUT(i),
-        PAD_EN => PAD_EN(i),
-        BDSR_IN => BDSR_IN(i),
-        BDSR_OUT => BDSR_OUT(i),
-        IOMODE => IOMODE,
-        SAMPLEMODE => SAMPLEMODE,
-        TCK => TCK
-      );
-  end generate;
-  BDSRCONN: for i in 0 to IOS-2 generate
-  begin
-    BDSR_IN(i) <= BDSR_OUT(i+1);
-  end generate;
-  BDSR_IN(IOS-1) <= TDI;
-
-  -- Set IOMODE
-  -- Currently SR_2Core or SR_Z are not used
-  IOMODE <= SR_2Pad when IR = CMD_EXTEST else
-            SR_Through;
-
-  -- Set SAMPLEMODE
-  ISSAMPLECMD <= (IR = CMD_SAMPLEPRELOAD or IR = CMD_EXTEST) and DRSTATE = '1';
-  SAMPLEMODE <= SR_Sample when ISSAMPLECMD and STATE = Capture else
-                SR_Update when ISSAMPLECMD and STATE = Update else
-                SR_Shift when ISSAMPLECMD and STATE = Shift else
-                SR_Normal;
-
-  TDO <= BDSR_OUT(0) when ISSAMPLECMD and STATE = Shift else
-         '0';
-  TDO_EN <= '1' when ISSAMPLECMD and STATE = Shift else
-            '0';
-end rtl;
diff --git a/rtl/vhdl/c4m_jtag_iocell.vhdl b/rtl/vhdl/c4m_jtag_iocell.vhdl
deleted file mode 100644
index 6a218a6..0000000
--- a/rtl/vhdl/c4m_jtag_iocell.vhdl
+++ /dev/null
@@ -1,101 +0,0 @@
--- An JTAG boundary scan for bidirectional I/O
-
-library ieee;
-use ieee.std_logic_1164.ALL;
-
-use work.c4m_jtag.ALL;
-
-entity c4m_jtag_iocell is
-  generic (
-    IR_WIDTH:   integer := 2
-  );
-  port (
-    -- core connections
-    CORE_IN:    out std_logic;
-    CORE_OUT:   in std_logic;
-    CORE_EN:    in std_logic;
-
-    -- pad connections
-    PAD_IN:     in std_logic;
-    PAD_OUT:    out std_logic;
-    PAD_EN:     out std_logic;
-
-    -- BD shift register
-    BDSR_IN:    in std_logic;
-    BDSR_OUT:   out std_logic;
-
-      -- Mode of I/O cell
-    IOMODE:     in SRIOMODE_TYPE;
-    SAMPLEMODE: in SRSAMPLEMODE_TYPE;
-    TCK:        in std_logic
-  );
-end c4m_jtag_iocell;
-
-architecture rtl of c4m_jtag_iocell is
-  signal SR_IOIN: std_logic;
-  signal SR_IOOUT: std_logic;
-  signal SR_IOEN: std_logic;
-
-  signal CORE_IN_BD: std_logic;
-  signal PAD_OUT_BD: std_logic;
-  signal PAD_EN_BD: std_logic;
-begin
-  with IOMODE select
-  CORE_IN <=
-    PAD_IN when SR_Through | SR_Z,
-    PAD_IN when SR_2Pad,
-    CORE_IN_BD when SR_2Core,
-    'X' when others;
-
-  with IOMODE select
-  PAD_OUT <=
-    CORE_OUT when SR_Through,
-    PAD_OUT_BD when SR_2Pad,
-    '0' when SR_2Core | SR_Z,  
-    'X' when others;
-
-  with IOMODE select
-  PAD_EN <=
-    CORE_EN when SR_Through,
-    PAD_EN_BD when SR_2Pad,
-    '0' when SR_2Core | SR_Z,
-    'X' when others;
-
-  process (TCK)
-  begin
-    -- Sampling of inputs and shifting of boundary scan SR needs to be done on
-    -- rising edge of TCK
-    if rising_edge(TCK) then
-      case SAMPLEMODE is
-        when SR_Sample =>
-          SR_IOIN <= PAD_IN;
-          SR_IOOUT <= CORE_OUT;
-          SR_IOEN <= CORE_EN;
-
-        when SR_Shift =>
-          SR_IOIN <= BDSR_IN;
-          SR_IOOUT <= SR_IOIN;
-          SR_IOEN <= SR_IOOUT;
-
-        when others =>
-          null;
-      end case;
-    end if;
-    
-    -- Update of output from boundary scan SR needs to be done on falling edge
-    -- of TCK
-    if falling_edge(TCK) then
-      case SAMPLEMODE is
-        when SR_Update =>
-          CORE_IN_BD <= SR_IOIN;
-          PAD_OUT_BD <= SR_IOOUT;
-          PAD_EN_BD <= SR_IOEN;
-          
-        when others =>
-          null;
-      end case;
-    end if;
-  end process;
-
-  BDSR_OUT <= SR_IOEN;
-end rtl;
diff --git a/rtl/vhdl/c4m_jtag_irblock.vhdl b/rtl/vhdl/c4m_jtag_irblock.vhdl
deleted file mode 100644
index 8be483a..0000000
--- a/rtl/vhdl/c4m_jtag_irblock.vhdl
+++ /dev/null
@@ -1,64 +0,0 @@
--- Handle the instruction register for the JTAG controller
-
-library ieee;
-use ieee.std_logic_1164.ALL;
-
-use work.c4m_jtag.ALL;
-
-entity c4m_jtag_irblock is
-  generic (
-    IR_WIDTH:   integer := 2
-  );
-  port (
-    -- needed TAP signals
-    TCK:        in std_logic;
-    TDI:        in std_logic;
-    TDO:        out std_logic;
-    TDO_EN:     out std_logic := '0';
-    
-    -- JTAG state
-    STATE:      in TAPSTATE_TYPE;
-    NEXT_STATE: in TAPSTATE_TYPE;
-    IRSTATE:    in std_logic;
-
-    -- instruction register
-    IR:         out std_logic_vector(IR_WIDTH-1 downto 0)
-  );
-end c4m_jtag_irblock;
-
-architecture rtl of c4m_jtag_irblock is
-  signal SHIFT_IR:      std_logic_vector(IR_WIDTH-1 downto 0);
-
-  constant CMD_IDCODE:  std_logic_vector(IR_WIDTH-1 downto 0) := c4m_jtag_cmd_idcode(IR_WIDTH);
-begin
-  process (TCK, STATE)
-  begin
-    if STATE = TestLogicReset then
-        SHIFT_IR <= (others => '0');
-        IR <= CMD_IDCODE;
-    elsif rising_edge(TCK) then
-      if IRSTATE = '1' then
-        case STATE is
-          when Capture =>
-            SHIFT_IR(1) <= '0';
-            SHIFT_IR(0) <= '1';
-
-          when Shift =>
-            SHIFT_IR(IR_WIDTH-2 downto 0) <= SHIFT_IR(IR_WIDTH-1 downto 1);
-            SHIFT_IR(IR_WIDTH-1) <= TDI;
-
-          when Update =>
-            IR <= SHIFT_IR;
-
-          when others =>
-            null;
-        end case;
-      end if;
-    end if;
-  end process;
-
-  TDO <= SHIFT_IR(0) when STATE = Shift and IRSTATE = '1' else
-         '0';
-  TDO_EN <= '1' when STATE = Shift and IRSTATE = '1' else
-            '0';
-end rtl;
diff --git a/rtl/vhdl/c4m_jtag_pkg.vhdl b/rtl/vhdl/c4m_jtag_pkg.vhdl
deleted file mode 100644
index 0fc0749..0000000
--- a/rtl/vhdl/c4m_jtag_pkg.vhdl
+++ /dev/null
@@ -1,225 +0,0 @@
--- Package of jtag support code from the Chips4Makers project
-library ieee;
-use ieee.std_logic_1164.ALL;
-
-package c4m_jtag is
-  type TAPSTATE_TYPE is (
-    TestLogicReset,
-    RunTestIdle,
-    SelectDRScan,
-    SelectIRScan,
-    Capture,
-    Shift,
-    Exit1,
-    Pause,
-    Exit2,
-    Update
-  );
-  type SRIOMODE_TYPE is (
-    SR_Through, -- Connect core signal to pad signals
-    SR_2Pad,    -- Connect BD to pad
-    SR_2Core,   -- Connect BD to core
-    SR_Z        -- pad is high impedance
-  );
-  type SRSAMPLEMODE_TYPE is (
-    SR_Normal,  -- No sampling or shifting
-    SR_Sample,  -- Sample IO state in BD SR on rising edge of TCK
-    SR_Update,  -- Update BD from SR on falling edge of TCK
-    SR_Shift    -- Shift the BD SR
-  );
-
-  component c4m_jtag_tap_fsm is
-    port (
-      -- The TAP signals
-      TCK:      in std_logic;
-      TMS:      in std_logic;
-      TRST_N:   in std_logic;
-
-      -- The state outputs
-      STATE:    out TAPSTATE_TYPE;
-      NEXT_STATE: out TAPSTATE_TYPE;
-      DRSTATE:  out std_logic;
-      IRSTATE:  out std_logic
-    );
-  end component c4m_jtag_tap_fsm;
-
-  component c4m_jtag_irblock is
-    generic (
-      IR_WIDTH: integer := 2
-    );
-    port (
-      -- needed TAP signals
-      TCK:      in std_logic;
-      TDI:      in std_logic;
-      TDO:      out std_logic;
-      TDO_EN:   out std_logic;
-    
-      -- JTAG state
-      STATE:    in TAPSTATE_TYPE;
-      NEXT_STATE: in TAPSTATE_TYPE;
-      IRSTATE:  in std_logic;
-
-      -- instruction register
-      IR:       out std_logic_vector(IR_WIDTH-1 downto 0)
-    );
-  end component c4m_jtag_irblock;
-  
-  component c4m_jtag_idblock is
-    generic (
-      IR_WIDTH:         integer := 2;
-    
-      -- The default MANUFACTURING ID is not representing a valid
-      -- manufacturer according to the JTAG standard
-      MANUFACTURER:     std_logic_vector(10 downto 0) := "10001111111";
-      PART_NUMBER:      std_logic_vector(15 downto 0) := "0000000000000001";
-      VERSION:          std_logic_vector(3 downto 0) := "0000"
-    );
-    port (
-      -- needed TAP signals
-      TCK:      in std_logic;
-      TDI:      in std_logic;
-      TDO:      out std_logic;
-      TDO_EN:   out std_logic;
-
-      -- JTAG state
-      STATE:    in TAPSTATE_TYPE;
-      NEXT_STATE: in TAPSTATE_TYPE;
-      DRSTATE:  in std_logic;
-
-      -- The instruction
-      IR:       in std_logic_vector(IR_WIDTH-1 downto 0)
-    );
-  end component c4m_jtag_idblock;
-
-  component c4m_jtag_iocell is
-    port (
-      -- core connections
-      CORE_IN:  out std_logic;
-      CORE_OUT: in std_logic;
-      CORE_EN:  in std_logic;
-
-      -- pad connections
-      PAD_IN:   in std_logic;
-      PAD_OUT:  out std_logic;
-      PAD_EN:   out std_logic;
-
-      -- BD shift register
-      BDSR_IN:  in std_logic;
-      BDSR_OUT: out std_logic;
-
-      -- Mode of I/O cell
-      IOMODE:   in SRIOMODE_TYPE;
-      SAMPLEMODE: in SRSAMPLEMODE_TYPE;
-      TCK:      in std_logic
-    );
-  end component c4m_jtag_iocell;
-  
-  component c4m_jtag_ioblock is
-    generic (
-      IR_WIDTH: integer := 2;
-      IOS:      integer := 1
-    );
-    port (
-      -- needed TAP signals
-      TCK:      in std_logic;
-      TDI:      in std_logic;
-      TDO:      out std_logic;
-      TDO_EN:   out std_logic;
-
-      -- JTAG state
-      STATE:    in TAPSTATE_TYPE;
-      NEXT_STATE: in TAPSTATE_TYPE;
-      DRSTATE:  in std_logic;
-
-      -- The instruction
-      IR:       in std_logic_vector(IR_WIDTH-1 downto 0);
-
-      -- The I/O access ports
-      CORE_OUT: in std_logic_vector(IOS-1 downto 0);
-      CORE_IN:  out std_logic_vector(IOS-1 downto 0);
-      CORE_EN:  in std_logic_vector(IOS-1 downto 0);
-
-      -- The pad connections
-      PAD_OUT:  out std_logic_vector(IOS-1 downto 0);
-      PAD_IN:   in std_logic_vector(IOS-1 downto 0);
-      PAD_EN:   out std_logic_vector(IOS-1 downto 0)
-    );
-  end component c4m_jtag_ioblock;
-
-  component c4m_jtag_tap_controller is
-    generic (
-      DEBUG:            boolean := false;
-
-      IR_WIDTH:         integer := 2;
-      IOS:              integer := 1;
-
-      -- The default MANUFACTURING ID is not representing a valid
-      -- manufacturer according to the JTAG standard
-      MANUFACTURER:     std_logic_vector(10 downto 0) := "10001111111";
-      PART_NUMBER:      std_logic_vector(15 downto 0) := "0000000000000001";
-      VERSION:          std_logic_vector(3 downto 0) := "0000"
-    );
-    port (
-      -- The TAP signals
-      TCK:      in std_logic;
-      TMS:      in std_logic;
-      TDI:      in std_logic;
-      TDO:      out std_logic;
-      TRST_N:   in std_logic;
-
-      -- The FSM state indicators
-      RESET:    out std_logic; -- In reset state
-      DRCAPTURE: out std_logic; -- In DR_Capture state
-      DRSHIFT:  out std_logic; -- In DR_Shift state
-      DRUPDATE: out std_logic; -- In DR_Update state
-
-      -- The Instruction Register
-      IR:       out std_logic_vector(IR_WIDTH-1 downto 0);
-
-      -- The I/O access ports
-      CORE_IN:  out std_logic_vector(IOS-1 downto 0);
-      CORE_EN:  in std_logic_vector(IOS-1 downto 0);
-      CORE_OUT: in std_logic_vector(IOS-1 downto 0);
-
-      -- The pad connections
-      PAD_IN:   in std_logic_vector(IOS-1 downto 0);
-      PAD_EN:   out std_logic_vector(IOS-1 downto 0);
-      PAD_OUT:  out std_logic_vector(IOS-1 downto 0)
-    );
-  end component c4m_jtag_tap_controller;
-
-  function c4m_jtag_cmd_idcode(width: integer) return std_logic_vector;
-  function c4m_jtag_cmd_bypass(width: integer) return std_logic_vector;
-  function c4m_jtag_cmd_samplepreload(width: integer) return std_logic_vector;
-  function c4m_jtag_cmd_extest(width: integer) return std_logic_vector;
-end c4m_jtag;
-
-package body c4m_jtag is
-  function c4m_jtag_cmd_bypass(width: integer) return std_logic_vector is
-    variable return_vector: std_logic_vector(width-1 downto 0);
-  begin
-    return_vector := (others => '1');
-    return return_vector;
-  end;
-
-  function c4m_jtag_cmd_idcode(width: integer) return std_logic_vector is
-    variable return_vector: std_logic_vector(width-1 downto 0);
-  begin
-    return_vector := (0 => '1', others => '0');
-    return return_vector;
-  end;
-
-  function c4m_jtag_cmd_samplepreload(width: integer) return std_logic_vector is
-    variable return_vector: std_logic_vector(width-1 downto 0);
-  begin
-    return_vector := (1 => '1', others => '0');
-    return return_vector;
-  end;
-
-  function c4m_jtag_cmd_extest(width: integer) return std_logic_vector is
-    variable return_vector: std_logic_vector(width-1 downto 0);
-  begin
-    return_vector := (others => '0');
-    return return_vector;
-  end;
-end package body;
diff --git a/rtl/vhdl/c4m_jtag_tap_controller.vhdl b/rtl/vhdl/c4m_jtag_tap_controller.vhdl
deleted file mode 100644
index 217982e..0000000
--- a/rtl/vhdl/c4m_jtag_tap_controller.vhdl
+++ /dev/null
@@ -1,154 +0,0 @@
--- A JTAG complient tap controller implementation
--- This is implemented based on the IEEE 1149.1 standard
-
-library ieee;
-use ieee.std_logic_1164.ALL;
-
-use work.c4m_jtag.ALL;
-
-entity c4m_jtag_tap_controller is
-  generic (
-    DEBUG:              boolean := false;
-
-    IR_WIDTH:           integer := 2;
-    IOS:                integer := 1;
-
-    MANUFACTURER:       std_logic_vector(10 downto 0) := "10001111111";
-    PART_NUMBER:        std_logic_vector(15 downto 0) := "0000000000000001";
-    VERSION:            std_logic_vector(3 downto 0) := "0000"
-  );
-  port (
-    -- The TAP signals
-    TCK:        in std_logic;
-    TMS:        in std_logic;
-    TDI:        in std_logic;
-    TDO:        out std_logic;
-    TRST_N:     in std_logic;
-
-    -- The FSM state indicators
-    RESET:      out std_logic;
-    DRCAPTURE:  out std_logic;
-    DRSHIFT:    out std_logic;
-    DRUPDATE:   out std_logic;
-
-    -- The Instruction Register
-    IR:         out std_logic_vector(IR_WIDTH-1 downto 0);
-
-    -- The I/O access ports
-    CORE_IN:    out std_logic_vector(IOS-1 downto 0);
-    CORE_EN:    in std_logic_vector(IOS-1 downto 0);
-    CORE_OUT:   in std_logic_vector(IOS-1 downto 0);
-
-    -- The pad connections
-    PAD_IN:     in std_logic_vector(IOS-1 downto 0);
-    PAD_EN:     out std_logic_vector(IOS-1 downto 0);
-    PAD_OUT:    out std_logic_vector(IOS-1 downto 0)
-  );
-end c4m_jtag_tap_controller;
-
-architecture rtl of c4m_jtag_tap_controller is
-  signal S_STATE:       TAPSTATE_TYPE;
-  signal S_NEXT_STATE:  TAPSTATE_TYPE;
-  signal S_IRSTATE:     std_logic;
-  signal S_DRSTATE:     std_logic;
-  signal S_IR:          std_logic_vector(IR_WIDTH-1 downto 0);
-
-  signal IR_TDO:        std_logic;
-  signal IR_TDO_EN:     std_logic;
-  signal ID_TDO:        std_logic;
-  signal ID_TDO_EN:     std_logic;
-  signal IO_TDO:        std_logic;
-  signal IO_TDO_EN:     std_logic;
-begin
-  IR <= S_IR;
-
-  RESET     <= '1' when S_STATE = TestLogicReset              else '0';
-  DRCAPTURE <= '1' when S_STATE = Capture and S_DRSTATE = '1' else '0';
-  DRSHIFT   <= '1' when S_STATE = Shift   and S_DRSTATE = '1' else '0';
-  DRUPDATE  <= '1' when S_STATE = Update  and S_DRSTATE = '1' else '0';
-
-  -- JTAG state machine
-  FSM:  c4m_jtag_tap_fsm
-    port map (
-      TCK => TCK,
-      TMS => TMS,
-      TRST_N => TRST_N,
-      STATE => S_STATE,
-      NEXT_STATE => S_NEXT_STATE,
-      DRSTATE => S_DRSTATE,
-      IRSTATE => S_IRSTATE
-    );
-
-  -- The instruction register
-  IRBLOCK: c4m_jtag_irblock
-    generic map (
-      IR_WIDTH => IR_WIDTH
-    )
-    port map (
-      TCK => TCK,
-      TDI => TDI,
-      TDO => IR_TDO,
-      TDO_EN => IR_TDO_EN,
-      STATE => S_STATE,
-      NEXT_STATE => S_NEXT_STATE,
-      IRSTATE => S_IRSTATE,
-      IR => S_IR
-    );
-
-  -- The ID
-  IDBLOCK: c4m_jtag_idblock
-    generic map (
-      IR_WIDTH => IR_WIDTH,
-      PART_NUMBER => PART_NUMBER,
-      MANUFACTURER => MANUFACTURER
-    )
-    port map (
-      TCK => TCK,
-      TDI => TDI,
-      TDO => ID_TDO,
-      TDO_EN => ID_TDO_EN,
-      STATE => S_STATE,
-      NEXT_STATE => S_NEXT_STATE,
-      DRSTATE => S_DRSTATE,
-      IR => S_IR
-    );
-  
-  -- The IOS
-  IOBLOCK: c4m_jtag_ioblock
-    generic map (
-      IR_WIDTH => IR_WIDTH,
-      IOS => IOS
-    )
-    port map (
-      TCK => TCK,
-      TDI => TDI,
-      TDO => IO_TDO,
-      TDO_EN => IO_TDO_EN,
-      STATE => S_STATE,
-      NEXT_STATE => S_NEXT_STATE,
-      DRSTATE => S_DRSTATE,
-      IR => S_IR,
-      CORE_OUT => CORE_OUT,
-      CORE_IN => CORE_IN,
-      CORE_EN => CORE_EN,
-      PAD_OUT => PAD_OUT,
-      PAD_IN => PAD_IN,
-      PAD_EN => PAD_EN
-    );
-
-  TDO <= IR_TDO when IR_TDO_EN = '1' else
-         ID_TDO when ID_TDO_EN = '1' else
-         IO_TDO when IO_TDO_EN = '1' else
-         '0';
-
-  CHECK_EN: if DEBUG generate
-    signal EN:  std_logic_vector(2 downto 0) := "000";
-  begin
-    EN <= IR_TDO_EN & ID_TDO_EN & IO_TDO_EN;
-    assert EN = "000" or EN = "100" or EN = "010" or EN = "001"
-      report "TDO conflict in c4m_jtag_tap_controller"
-      severity ERROR;
-  end generate CHECK_EN;
-end rtl;
-
-
diff --git a/rtl/vhdl/c4m_jtag_tap_fsm.vhdl b/rtl/vhdl/c4m_jtag_tap_fsm.vhdl
deleted file mode 100644
index 005eccb..0000000
--- a/rtl/vhdl/c4m_jtag_tap_fsm.vhdl
+++ /dev/null
@@ -1,140 +0,0 @@
--- The JTAG state machine
--- This is implemented based on the IEEE 1149.1 standard
-
-library ieee;
-use ieee.std_logic_1164.ALL;
-
-use work.c4m_jtag.ALL;
-
-entity c4m_jtag_tap_fsm is
-  port (
-    -- The TAP signals
-    TCK:        in std_logic;
-    TMS:        in std_logic;
-    TRST_N:     in std_logic;
-
-    -- The state outputs
-    STATE:      out TAPSTATE_TYPE;
-    NEXT_STATE: out TAPSTATE_TYPE;
-    DRSTATE:    out std_logic;
-    IRSTATE:    out std_logic
-  );
-end c4m_jtag_tap_fsm;
-
-architecture rtl of c4m_jtag_tap_fsm is
-  signal S_STATE:         TAPSTATE_TYPE;
-  signal S_NEXT_STATE:    TAPSTATE_TYPE;
-  signal S_DRSTATE:       std_logic;
-  signal S_IRSTATE:       std_logic;
-  signal NEXT_DRSTATE:    std_logic;
-  signal NEXT_IRSTATE:    std_logic;
-begin
-  STATE <= S_STATE;
-  NEXT_STATE <= S_NEXT_STATE;
-  DRSTATE <= S_DRSTATE;
-  IRSTATE <= S_IRSTATE;
-  
-  process (TCK, TRST_N)
-  begin
-    if TRST_N = '0' then
-      S_DRSTATE <= '0';
-      S_IRSTATE <= '0';
-      S_STATE <= TestLogicReset;
-    elsif rising_edge(TCK) then
-      S_STATE <= S_NEXT_STATE;
-      S_DRSTATE <= NEXT_DRSTATE;
-      S_IRSTATE <= NEXT_IRSTATE;
-    end if;
-  end process;
-
-  NEXT_DRSTATE <=
-    '0' when S_NEXT_STATE = TestLogicReset else
-    '0' when S_NEXT_STATE = RunTestIdle else
-    '1' when S_NEXT_STATE = SelectDRScan else
-    '0' when S_NEXT_STATE = SelectIRScan else
-    S_DRSTATE;
-  NEXT_IRSTATE <=
-    '0' when S_NEXT_STATE = TestLogicReset else
-    '0' when S_NEXT_STATE = RunTestIdle else
-    '0' when S_NEXT_STATE = SelectDRScan else
-    '1' when S_NEXT_STATE = SelectIRScan else
-    S_IRSTATE;
-
-  process (S_STATE, TMS)
-  begin
-    case S_STATE is
-      when TestLogicReset =>
-        if (TMS = '0') then
-          S_NEXT_STATE <= RunTestIdle;
-        else
-          S_NEXT_STATE <= TestLogicReset;
-        end if;
-
-      when RunTestIdle =>
-        if (TMS = '0') then
-          S_NEXT_STATE <= RunTestIdle;
-        else
-          S_NEXT_STATE <= SelectDRScan;
-        end if;
-
-      when SelectDRScan =>
-        if (TMS = '0') then
-          S_NEXT_STATE <= Capture;
-        else
-          S_NEXT_STATE <= SelectIRScan;
-        end if;
-
-      when SelectIRScan =>
-        if (TMS = '0') then
-          S_NEXT_STATE <= Capture;
-        else
-          S_NEXT_STATE <= TestLogicReset;
-        end if;
-
-      when Capture =>
-        if (TMS = '0') then
-          S_NEXT_STATE <= Shift;
-        else
-          S_NEXT_STATE <= Exit1;
-        end if;
-
-      when Shift =>
-        if (TMS = '0') then
-          S_NEXT_STATE <= Shift;
-        else
-          S_NEXT_STATE <= Exit1;
-        end if;
-
-      when Exit1 =>
-        if (TMS = '0') then
-          S_NEXT_STATE <= Pause;
-        else
-          S_NEXT_STATE <= Update;
-        end if;
-
-      when Pause =>
-        if (TMS = '0') then
-          S_NEXT_STATE <= Pause;
-        else
-          S_NEXT_STATE <= Exit2;
-        end if;
-
-      when Exit2 =>
-        if (TMS = '0') then
-          S_NEXT_STATE <= Shift;
-        else
-          S_NEXT_STATE <= Update;
-        end if;
-
-      when Update =>
-        if (TMS = '0') then
-          S_NEXT_STATE <= RunTestIdle;
-        else
-          S_NEXT_STATE <= SelectDRScan;
-        end if;
-
-      when others =>
-        S_NEXT_STATE <= TestLogicReset;
-    end case;
-  end process;
-end rtl;
diff --git a/sim/cocotb/c4m_jtag.py b/sim/cocotb/c4m_jtag.py
deleted file mode 100644
index 1dd13b1..0000000
--- a/sim/cocotb/c4m_jtag.py
+++ /dev/null
@@ -1,210 +0,0 @@
-import cocotb
-from cocotb.triggers import Timer
-from cocotb.binary import BinaryValue
-
-class JTAGException(Exception):
-    pass
-
-class JTAG_Clock(object):
-    """
-    Class for the JTAG clock, run cycle by cycle
-    """
-    def __init__(self, signal, period):
-        self.signal = signal
-        self.t = Timer(period/4)
-
-    @cocotb.coroutine
-    def Cycle(self, cycles=1):
-        """
-        Do one or more cycles
-        Cycle start in middle of 0 pulse of the clock
-        """
-        for i in range(cycles):
-            self.signal <= 0
-            yield self.t
-            self.signal <= 1
-            yield self.t
-            yield self.t
-            self.signal <= 0
-            yield self.t
-
-class JTAG_Master(object):
-    """
-    Class that will run JTAG commands, shift in and out data
-    """
-    #TODO: Handle a JTAG chain with more than one device
-
-    def __init__(self, tck, tms, tdi, tdo, trst_n=None, clk_period=1000):
-        self.tck = tck
-        self.clkgen = JTAG_Clock(tck, clk_period)
-        tck <= 0
-        self.tms = tms
-        tms <= 1
-        self.tdi = tdi
-        tdi <= 0
-        self.tdo = tdo
-        self.trst_n = trst_n
-        if trst_n is not None:
-            trst_n <= 1
-        self.period = Timer(clk_period)
-
-        # Standard commands
-        # TODO: make IR length configurable; now 2 is assumed
-        self.BYPASS = [1, 1]
-        self.IDCODE = [0, 1]
-        self.SAMPLEPRELOAD = [1, 0]
-        self.EXTEST = [0, 0]
-
-        # After command we always leave the controller in reset or runidle state
-        # If value is None we will always reset the interface
-        self.state = None
-
-        # The methods of this class are coroutines. The results will be stored
-        # in the result field
-        self.result = None
-
-    @cocotb.coroutine
-    def cycle_clock(self, cycles=1):
-        if self.state == "Run" and self.tms == 1:
-            self.state = "Scan"
-        yield self.clkgen.Cycle(cycles)
-
-    @cocotb.coroutine
-    def reset(self):
-        if not self.trst_n is None:
-            # Enable reset signal for one clock period
-            self.trst_n <= 0
-            yield self.period
-            self.trst_n <= 1
-        else:
-            # 5 cycles with tms on 1 should reset the JTAG TAP controller
-            self.tms <= 1
-            yield self.cycle_clock(5)
-
-        self.state = "Reset"
-
-        self.result = None
-
-    @cocotb.coroutine
-    def change_state(self, tms_list):
-        """
-        Put TAP in other state by giving a TMS sequence
-        This function does not detect if one ends up in reset or run
-        state afterwards, self.state has to be updated by caller
-        if that is the case.
-        """
-        tms_copy = list(tms_list)
-        while tms_copy:
-            self.tms <= tms_copy.pop()
-            yield self.cycle_clock()
-        self.result = None
-
-    @cocotb.coroutine
-    def change_to_run(self):
-        """
-        Put TAP in RunTestIdle state
-        self.result is bool and true if TAP went through reset state
-        """
-        isreset = False
-        if self.state is None:
-            yield self.reset()
-        if self.state is "Reset":
-            isreset = True
-            self.tms <= 0
-            yield self.cycle_clock()
-            self.state = "Run"
-        assert(self.state == "Run")
-        self.result = isreset
-
-    @cocotb.coroutine
-    def load_ir(self, cmd):
-        cmd_copy = list(cmd)
-        result = BinaryValue(bits=len(cmd_copy))
-        l_result = list()
-
-        yield self.change_to_run()
-        # Go to Capture/IR
-        yield self.change_state([0, 1, 1])
-
-        # Shift the two
-        self.tms <= 0
-        while cmd_copy:
-            # In first iteration we enter SHIFT state and tdo is made active
-            yield self.cycle_clock()
-            # For the last iteration tdi will be shifted when entering next state
-            self.tdi <= cmd_copy.pop()
-            l_result.insert(0, str(self.tdo))
-
-        # Go to RunTestIdle
-        yield self.change_state([0, 1, 1])
-        self.state = "Run"
-
-    @cocotb.coroutine
-    def idcode(self):
-        """
-        Get the IDCODE from the device
-        result will contain the 32 bit IDCODE of the device
-        """
-
-        result = BinaryValue(bits=32)
-        l_result = list()
-
-        # Keep tdi 0 for the whole run
-        self.tdi <= 0
-
-        yield self.change_to_run()
-        if not self.result:
-            # If TAP was not reset we have to load IDCODE command
-            yield self.load_ir(self.IDCODE)
-
-        # Should be again in RUN state
-        assert(self.state == "Run")
-
-        # Go to Shift/DR
-        yield self.change_state([0, 0, 1])
-
-        # Enter Shift; run for 32 cycles
-        self.tms <= 0
-        for i in range(32):
-            l_result.insert(0, str(self.tdo))
-            yield self.cycle_clock()
-        result.binstr = "".join(l_result)
-
-        # Go to RunTestIdle
-        yield self.change_state([0, 1, 1])
-        self.state = "Run"
-
-        self.result = result
-
-    @cocotb.coroutine
-    def shift_data(self, data_in):
-        """
-        Shift data in through the JTAG and capture the output
-        Input can be of type BinaryValue or an iterable value of 0 and 1s.
-        Last bit will be shifted in first.
-        result will contain the sample TDO with the same number of bits as the input
-        """
-        if isinstance(data_in, BinaryValue):
-            data_copy = [int(c) for c in data_in.binstr]
-        else:
-            data_copy = list(data_in)
-        result = BinaryValue()
-        l_result = list()
-
-        yield self.change_to_run()
-        # Go to Capture/DR
-        yield self.change_state([0, 1])
-
-        # Shift data through
-        self.tms <= 0
-        while data_copy:
-            yield self.cycle_clock()
-            self.tdi <= data_copy.pop()
-            l_result.insert(0, str(self.tdo))
-        result.binstr = "".join(l_result)
-
-        # Go to RunTestIdle
-        yield self.change_state([0, 1, 1])
-        self.state = "Run"
-
-        self.result = result
diff --git a/sim/cocotb/c4m_jtag_svfcocotb.py b/sim/cocotb/c4m_jtag_svfcocotb.py
deleted file mode 100755
index 177b7a1..0000000
--- a/sim/cocotb/c4m_jtag_svfcocotb.py
+++ /dev/null
@@ -1,226 +0,0 @@
-import c4m_jtag_svfgrammar
-import cocotb
-from cocotb.binary import BinaryValue
-from functools import singledispatch
-
-def decodescanspec(node):
-    length = int(str(node[2]))
-    fstr = "{:0"+str(node[2])+"b}"
-
-    g_tdi = node[4]
-    g_tdo = node[5]
-    g_mask = node[6]
-    g_smask = node[7]
-
-    if g_tdi is None:
-        tdi = None
-    else:
-        tdi = BinaryValue(fstr.format(int(str(g_tdi[2]),16)), length)
-
-    if g_tdo is None:
-        tdo = None
-    else:
-        tdo = BinaryValue(fstr.format(int(str(g_tdo[3]),16)), length)
-
-    if g_mask is None:
-        mask = None
-    else:
-        mask = BinaryValue(fstr.format(int(str(g_mask[3]),16)), length)
-
-    if g_smask is None:
-        smask = None
-    else:
-        smask = BinaryValue(fstr.format(int(str(g_smask[3]),16)), length)
-
-    return (length, tdi, tdo, mask, smask)
-
-
-class SVF_Executor(object):
-    @cocotb.coroutine
-    def execute(self, node):
-        """This is the generic method"""
-        self._p("generic")
-        if False: # Make coroutine work
-            yield PythonTrigger()
-
-    @cocotb.coroutine
-    def _execute_NOP(self, node):
-        if False: # Make coroutine work
-            yield PythonTrigger()
-
-    @cocotb.coroutine
-    def _execute_EndDR(self, node):
-        self._p("EndDR ignored")
-        if False: # Make coroutine work
-            yield PythonTrigger()
-
-    @cocotb.coroutine
-    def _execute_EndIR(self, node):
-        self._p("EndIR ignored")
-        if False: # Make coroutine work
-            yield PythonTrigger()
-
-    @cocotb.coroutine
-    def _execute_Frequency(self, node):
-        self._p("Frequency ignored")
-        if False: # Make coroutine work
-            yield PythonTrigger()
-
-    @cocotb.coroutine
-    def _execute_HDR(self, node):
-        self._p("HDR ignored")
-        if False: # Make coroutine work
-            yield PythonTrigger()
-
-    @cocotb.coroutine
-    def _execute_HIR(self, node):
-        self._p("HIR ignored")
-        if False: # Make coroutine work
-            yield PythonTrigger()
-
-    @cocotb.coroutine
-    def _execute_SDR(self, node):
-        self._p("Executing SDR")
-        (length, tdi, tdo, mask, smask) = decodescanspec(node)
-
-        samelength = length == self._d_length
-        self._d_length = length
-
-        if tdi is None:
-            if not samelength:
-                raise(JTAGException("TDI needs to be specified when length of data changes"))
-        else:
-            self._d_tdi = tdi
-
-        if mask is not None:
-            self._d_mask = mask
-        elif not samelength:
-            self._d_mask = None
-
-        if smask is not None:
-            self._d_smask = smask
-        elif not samelength:
-            self._d_smask = None
-
-        yield self.master.shift_data(self._d_tdi)
-        if tdo is not None:
-            if self._d_mask is not None:
-                raise(JTAGException("MASK not supported for SDR"))
-            assert(self.result == tdo)
-
-    @cocotb.coroutine
-    def _execute_SIR(self, node):
-        (length, tdi, tdo, mask, smask) = decodescanspec(node)
-
-        samelength = length == self._i_length
-        self._i_length = length
-
-        if tdi is None:
-            if not samelength:
-                raise(JTAGException("TDI needs to be specified when length of data changes"))
-        else:
-            self._i_tdi = tdi
-
-        if mask is not None:
-            self._i_mask = mask
-        elif not samelength:
-            self._i_mask = None
-
-        if smask is not None:
-            self._i_smask = smask
-        elif not samelength:
-            self._i_smask = None
-
-        self._p("Executing SIR ({})".format(self._i_tdi.integer))
-
-        yield self.master.load_ir(self._i_tdi)
-        if tdo is not None:
-            if self._i_mask is not None:
-                raise(JTAGException("MASK not supported for SIR"))
-            assert(self.result == tdo)
-        
-
-    @cocotb.coroutine
-    def _execute_State(self, node):
-        self._p("State")
-        if False: # Make coroutine work
-            yield PythonTrigger()
-
-    @cocotb.coroutine
-    def _execute_TDR(self, node):
-        self._p("TDR")
-        if False: # Make coroutine work
-            yield PythonTrigger()
-
-    @cocotb.coroutine
-    def _execute_TIR(self, node):
-        self._p("TIR")
-        if False: # Make coroutine work
-            yield PythonTrigger()
-
-    @cocotb.coroutine
-    def _execute_Trst(self, node):
-        self._p("TRST ignored")
-        if False: # Make coroutine work
-            yield PythonTrigger()
-
-    @cocotb.coroutine
-    def _execute_Runtest(self, node):
-        if node[1] is not None:
-            raise(JTAGException("State specification for RUNTEST not supported"))
-        # TODO: cycle the right number of clocks or wait the right time
-        yield(self.master.change_state([0]))
-
-    @cocotb.coroutine
-    def _execute_SVFFile(self, node):
-        self._p("Executing SVFFile")
-        for statement in node.elements[0]:
-            yield self.execute(statement)
-
-    def __init__(self, master):
-        # master is assumed to be a JTAG_Master class
-        # it needs to support methods load_ir() and shift_data()
-        self.master = master
-
-        # Due to bug in Grammar definition all possible classes have to have
-        # a dispatch entry otherwise an error will be raised.
-        self.execute = singledispatch(self.execute)
-        self.execute.register(c4m_jtag_svfgrammar.EmptyLine, self._execute_NOP)
-        self.execute.register(c4m_jtag_svfgrammar.Comment, self._execute_NOP)
-        self.execute.register(c4m_jtag_svfgrammar.EndDR, self._execute_EndDR)
-        self.execute.register(c4m_jtag_svfgrammar.EndIR, self._execute_EndIR)
-        self.execute.register(c4m_jtag_svfgrammar.Frequency, self._execute_Frequency)
-        self.execute.register(c4m_jtag_svfgrammar.HDR, self._execute_HDR)
-        self.execute.register(c4m_jtag_svfgrammar.HIR, self._execute_HIR)
-        self.execute.register(c4m_jtag_svfgrammar.Runtest, self._execute_Runtest)
-        self.execute.register(c4m_jtag_svfgrammar.SDR, self._execute_SDR)
-        self.execute.register(c4m_jtag_svfgrammar.SIR, self._execute_SIR)
-        self.execute.register(c4m_jtag_svfgrammar.State, self._execute_State)
-        self.execute.register(c4m_jtag_svfgrammar.TDR, self._execute_TDR)
-        self.execute.register(c4m_jtag_svfgrammar.TIR, self._execute_TIR)
-        self.execute.register(c4m_jtag_svfgrammar.Trst, self._execute_Trst)
-        self.execute.register(c4m_jtag_svfgrammar.SVFFile, self._execute_SVFFile)
-
-        # Store the head and tail for the scan
-        self._d_tdi = self._d_tdi_h = self._d_tdi_t = None
-        self._d_tdo_h = self._d_tdo_t = None
-        self._i_tdi = self._i_tdi_h = self._i_tdi_t = None
-        self._i_tdo_h = self._i_tdo_t = None
-
-        # Remember the masks; smasks are ignored and bits always considered as care, e.g right
-        # value applied
-        self._d_length = self._d_length_h = self._d_length_t = None
-        self._d_mask = self._d_mask_h = self._d_mask_t = None
-        self._d_smask = self._d_smask_h = self._d_smask_t = None
-        self._i_length = self._i_length_h = self._i_length_t = None
-        self._i_mask = self._i_mask_h = self._i_mask_t = None
-        self._i_smask = self._i_smask_h = self._i_smask_t = None
-
-    @cocotb.coroutine
-    def run(self, cmds, p=print):
-        self._p = p
-        if isinstance(cmds, c4m_jtag_svfgrammar.SVFFile):
-            yield self.execute(cmds)
-        else:
-            p = c4m_jtag_svfgrammar.SVFFile.parser()
-            yield self.execute(p.parse_string(cmds))
diff --git a/sim/cocotb/c4m_jtag_svfgrammar.py b/sim/cocotb/c4m_jtag_svfgrammar.py
deleted file mode 100644
index faf3aeb..0000000
--- a/sim/cocotb/c4m_jtag_svfgrammar.py
+++ /dev/null
@@ -1,96 +0,0 @@
-from modgrammar import *
-
-grammar_whitespace_mode = 'explicit'
-grammar_whitespace = WS_NOEOL
-
-class SVFEol(Grammar):
-    grammar = (OPTIONAL(SPACE), EOL)
-    grammar_collapse = True
-
-class SemicolonEol(Grammar):
-    grammar = (OPTIONAL(SPACE), L(";"), SVFEol)
-    grammar_collapse = True
-
-class Integer(Grammar):
-    grammar = WORD("0-9")
-    grammar_collapse = True
-
-class Float(Grammar):
-    grammar = (Integer, (L("."), OPTIONAL(Integer)), OPTIONAL(L("E"), Integer))
-
-class Hexadecimal(Grammar):
-    grammar = (L("("), WORD("0-9A-F"), L(")"))
-    grammar_collapse = True
-
-class StableState(Grammar):
-    grammar = (L("IRPAUSE") | L("DRPAUSE") | L("RESET") | L("IDLE"))
-    grammar_collapse = True
-
-
-class ScanSpec(Grammar):
-    """The specification of Scan In/Scan out data"""
-    grammar = (SPACE, Integer, SPACE,
-               OPTIONAL(L("TDI"), OPTIONAL(SPACE), Hexadecimal),
-               OPTIONAL(OPTIONAL(SPACE), L("TDO"), OPTIONAL(SPACE), Hexadecimal),
-               OPTIONAL(OPTIONAL(SPACE), L("MASK"), OPTIONAL(SPACE), Hexadecimal),
-               OPTIONAL(OPTIONAL(SPACE), L("SMASK"), OPTIONAL(SPACE), Hexadecimal)
-              )
-    grammar_collapse = True
-
-
-class EmptyLine(Grammar):
-    grammar = SVFEol
-
-class Comment(Grammar):
-    grammar = (L("!"), REST_OF_LINE, SVFEol)
-
-class EndDR(Grammar):
-    grammar = (L("ENDDR"), SPACE, StableState, SemicolonEol)
-
-class EndIR(Grammar):
-    grammar = (L("ENDIR"), SPACE, StableState, SemicolonEol)
-
-class Frequency(Grammar):
-    grammar = (L("FREQUENCY"), OPTIONAL(SPACE, Float, OPTIONAL(SPACE), L("HZ")), OPTIONAL(SPACE), SemicolonEol)
-
-class HDR(Grammar):
-    grammar = (L("HDR"), ScanSpec, SemicolonEol)
-
-class HIR(Grammar):
-    grammar = (L("HIR"), ScanSpec, SemicolonEol)
-
-#TODO: PIO, PIOMAP
-
-class Runtest(Grammar):
-    grammar = (
-        L("RUNTEST"),
-        OPTIONAL(SPACE, StableState),
-        OPTIONAL(SPACE, Integer, OPTIONAL(SPACE), (L("TCK") | L("SCK"))),
-        OPTIONAL(SPACE, Float, OPTIONAL(SPACE), L("SEC")),
-        OPTIONAL(SPACE, L("MAXIMUM"), SPACE, Float, OPTIONAL(SPACE), L("SEC")),
-        OPTIONAL(SPACE, L("ENDSTATE"), SPACE, StableState),
-        SemicolonEol
-    )
-
-class SDR(Grammar):
-    grammar = (L("SDR"), ScanSpec, SemicolonEol)
-
-class SIR(Grammar):
-    grammar = (L("SIR"), ScanSpec, SemicolonEol)
-
-class State(Grammar):
-    # TODO: Path to reach state
-    grammar = (L("STATE"), SPACE, StableState, SemicolonEol)
-
-class TDR(Grammar):
-    grammar = (L("TDR"), ScanSpec, SemicolonEol)
-
-class TIR(Grammar):
-    grammar = (L("TIR"), ScanSpec, SemicolonEol)
-
-class Trst(Grammar):
-    grammar = (L("TRST"), SPACE, (L("ON") | L("OFF") | L("Z") | L("ABSENT")), SemicolonEol)
-
-
-class SVFFile(Grammar):
-    grammar = ONE_OR_MORE(EmptyLine | Comment | Trst | EndDR | EndIR | SIR | SDR | Runtest | State)
diff --git a/sim/cocotb/controller/Makefile b/sim/cocotb/controller/Makefile
deleted file mode 100644
index 708e50d..0000000
--- a/sim/cocotb/controller/Makefile
+++ /dev/null
@@ -1,22 +0,0 @@
-PWD=$(realpath .)
-TOPDIR=$(realpath ../../..)
-VHDLDIR=$(TOPDIR)/rtl/vhdl
-VHDL_SOURCES = \
-  $(VHDLDIR)/c4m_jtag_pkg.vhdl \
-  $(VHDLDIR)/c4m_jtag_tap_fsm.vhdl \
-  $(VHDLDIR)/c4m_jtag_irblock.vhdl \
-  $(VHDLDIR)/c4m_jtag_iocell.vhdl \
-  $(VHDLDIR)/c4m_jtag_ioblock.vhdl \
-  $(VHDLDIR)/c4m_jtag_idblock.vhdl \
-  $(VHDLDIR)/c4m_jtag_tap_controller.vhdl
-TOPLEVEL=c4m_jtag_tap_controller
-TOPLEVEL_LANG=vhdl
-MODULE=test
-SIM=ghdl
-GPI_IMPL=vhpi
-SIM_ARGS=--wave=test.ghw
-
-COCOTBDIR=$(shell cocotb-path)
-
-include $(COCOTBDIR)/makefiles/Makefile.inc
-include $(COCOTBDIR)/makefiles/Makefile.sim
diff --git a/sim/cocotb/controller/c4m_jtag.py b/sim/cocotb/controller/c4m_jtag.py
deleted file mode 120000
index f409baa..0000000
--- a/sim/cocotb/controller/c4m_jtag.py
+++ /dev/null
@@ -1 +0,0 @@
-../c4m_jtag.py
\ No newline at end of file
diff --git a/sim/cocotb/controller/test.py b/sim/cocotb/controller/test.py
deleted file mode 100644
index 9d66ea8..0000000
--- a/sim/cocotb/controller/test.py
+++ /dev/null
@@ -1,94 +0,0 @@
-import cocotb
-from cocotb.utils import get_sim_steps
-from cocotb.binary import BinaryValue
-
-from c4m_jtag import JTAG_Master
-
-@cocotb.test()
-def test01_idcode(dut):
-    """
-    Test the IDCODE command
-    """
-
-    # Run @ 1MHz
-    clk_period = get_sim_steps(1, "us")
-    master = JTAG_Master(dut.tck, dut.tms, dut.tdi, dut.tdo, dut.trst_n, clk_period)
-
-    dut._log.info("Trying to get IDCODE...")
-
-    yield master.idcode()
-    result1 = master.result
-    dut._log.info("IDCODE1: {}".format(result1))
-
-    yield master.idcode()
-    result2 = master.result
-    dut._log.info("IDCODE2: {}".format(result2))
-
-    assert(result1 == result2)
-    
-@cocotb.test()
-def test02_bypass(dut):
-    """
-    Test of BYPASS mode
-    """
-
-    # Run @ 1MHz
-    clk_period = get_sim_steps(1, "us")
-    master = JTAG_Master(dut.tck, dut.tms, dut.tdi, dut.tdo, dut.trst_n, clk_period)
-
-    dut._log.info("Loading BYPASS command")
-    yield master.load_ir(master.BYPASS)
-
-    dut._log.info("Sending data")
-
-    data_in = BinaryValue()
-    data_in.binstr = "01001101"
-    yield master.shift_data(data_in)
-
-    dut._log.info("bypass out: {}".format(master.result.binstr))
-    assert(master.result.binstr[:-1] == data_in.binstr[1:])
-
-@cocotb.test()
-def test03_sample(dut):
-    """
-    Test of SAMPLEPRELOAD and EXTEST
-    """
-    data_in = BinaryValue()
-
-    # Run @ 1MHz
-    clk_period = get_sim_steps(1, "us")
-    master = JTAG_Master(dut.tck, dut.tms, dut.tdi, dut.tdo, dut.trst_n, clk_period)
-
-
-    dut._log.info("Load SAMPLEPRELOAD command")
-    yield master.load_ir(master.SAMPLEPRELOAD)
-
-    data_in.binstr = "011"
-    dut._log.info("  preloading data {}".format(data_in.binstr))
-
-    # Set the ios pins
-    dut.core_out = 0
-    dut.core_en = 0
-    dut.pad_in = 1
-    yield master.shift_data(data_in)
-    dut._log.info("  output: {}".format(master.result.binstr))
-    assert(master.result.binstr == "100")
-
-
-    dut._log.info("Load EXTEST command")
-    yield master.load_ir(master.EXTEST)
-
-    data_in.binstr = "100"
-    dut._log.info("  input data {}".format(data_in.binstr))
-    
-    # Set the ios pins
-    dut.core_out = 1
-    dut.core_en = 1
-    dut.pad_in = 0
-    yield master.shift_data(data_in)
-    dut._log.info("  output: {}".format(master.result.binstr))
-    assert(master.result.binstr == "011")
-
-    dut._log.info("Do a capture of the last loaded data")
-    yield master.shift_data([])
-
diff --git a/sim/cocotb/dual_parallel/Makefile b/sim/cocotb/dual_parallel/Makefile
deleted file mode 100644
index e7da7cd..0000000
--- a/sim/cocotb/dual_parallel/Makefile
+++ /dev/null
@@ -1,23 +0,0 @@
-PWD=$(realpath .)
-TOPDIR=$(realpath ../../..)
-VHDLDIR=$(TOPDIR)/rtl/vhdl
-VHDL_SOURCES = \
-  $(VHDLDIR)/c4m_jtag_pkg.vhdl \
-  $(VHDLDIR)/c4m_jtag_tap_fsm.vhdl \
-  $(VHDLDIR)/c4m_jtag_irblock.vhdl \
-  $(VHDLDIR)/c4m_jtag_iocell.vhdl \
-  $(VHDLDIR)/c4m_jtag_ioblock.vhdl \
-  $(VHDLDIR)/c4m_jtag_idblock.vhdl \
-  $(VHDLDIR)/c4m_jtag_tap_controller.vhdl \
-  $(PWD)/dual_parallel.vhdl
-TOPLEVEL=dual_parallel
-TOPLEVEL_LANG=vhdl
-MODULE=test
-SIM=ghdl
-GPI_IMPL=vhpi
-SIM_ARGS=--wave=test.ghw
-
-COCOTBDIR=$(shell cocotb-path)
-
-include $(COCOTBDIR)/makefiles/Makefile.inc
-include $(COCOTBDIR)/makefiles/Makefile.sim
diff --git a/sim/cocotb/dual_parallel/c4m_jtag.py b/sim/cocotb/dual_parallel/c4m_jtag.py
deleted file mode 120000
index f409baa..0000000
--- a/sim/cocotb/dual_parallel/c4m_jtag.py
+++ /dev/null
@@ -1 +0,0 @@
-../c4m_jtag.py
\ No newline at end of file
diff --git a/sim/cocotb/dual_parallel/dual_parallel.vhdl b/sim/cocotb/dual_parallel/dual_parallel.vhdl
deleted file mode 100644
index 989d15a..0000000
--- a/sim/cocotb/dual_parallel/dual_parallel.vhdl
+++ /dev/null
@@ -1,82 +0,0 @@
--- Top cell with two instantiations of the tap_controller with parallel scan chains
-
-library ieee;
-use ieee.std_logic_1164.ALL;
-
-use work.c4m_jtag.ALL;
-
-entity dual_parallel is
-  port (
-    -- Instance 1
-    -- ==========
-    -- JTAG
-    I1_TCK:     in std_logic;
-    I1_TMS:     in std_logic;
-    I1_TDI:     in std_logic;
-    I1_TDO:     out std_logic;
-    I1_TRST_N:  in std_logic;
-
-    -- Instance 2
-    -- ==========
-    -- JTAG
-    I2_TCK:     in std_logic;
-    I2_TMS:     in std_logic;
-    I2_TDI:     in std_logic;
-    I2_TDO:     out std_logic;
-    I2_TRST_N:  in std_logic
-  );
-end dual_parallel;
-
-architecture rtl of dual_parallel is
-  signal I1_PAD_IN:     std_logic;
-  signal I1_PAD_EN:     std_logic;
-  signal I1_PAD_OUT:    std_logic;
-  signal I2_PAD_IN:     std_logic;
-  signal I2_PAD_EN:     std_logic;
-  signal I2_PAD_OUT:    std_logic;
-begin
-  CTRL1: c4m_jtag_tap_controller
-    port map (
-      TCK => I1_TCK,
-      TMS => I1_TMS,
-      TDI => I1_TDI,
-      TDO => I1_TDO,
-      TRST_N => I1_TRST_N,
-      RESET => open,
-      DRCAPTURE => open,
-      DRSHIFT => open,
-      DRUPDATE => open,
-      IR => open,
-      CORE_IN => open,
-      CORE_EN => "1",
-      CORE_OUT => "1",
-      PAD_IN(0) => I1_PAD_IN,
-      PAD_EN(0) => I1_PAD_EN,
-      PAD_OUT(0) => I1_PAD_OUT
-    );
-
-  CTRL2: c4m_jtag_tap_controller
-    port map (
-      TCK => I2_TCK,
-      TMS => I2_TMS,
-      TDI => I2_TDI,
-      TDO => I2_TDO,
-      TRST_N => I2_TRST_N,
-      RESET => open,
-      DRCAPTURE => open,
-      DRSHIFT => open,
-      DRUPDATE => open,
-      IR => open,
-      CORE_IN => open,
-      CORE_EN => "1",
-      CORE_OUT => "0",
-      PAD_IN(0) => I2_PAD_IN,
-      PAD_EN(0) => I2_PAD_EN,
-      PAD_OUT(0) => I2_PAD_OUT
-    );
-
-  I1_PAD_IN <= I2_PAD_OUT when I2_PAD_EN = '1' else
-               'Z';
-  I2_PAD_IN <= I1_PAD_OUT when I1_PAD_EN = '1' else
-               'Z';
-end rtl;
diff --git a/sim/cocotb/dual_parallel/test.py b/sim/cocotb/dual_parallel/test.py
deleted file mode 100644
index 47cf306..0000000
--- a/sim/cocotb/dual_parallel/test.py
+++ /dev/null
@@ -1,41 +0,0 @@
-import cocotb
-from cocotb.utils import get_sim_steps
-
-from c4m_jtag import JTAG_Master
-
-@cocotb.test()
-def test01_dual(dut):
-    """
-    Test the IDCODE command
-    """
-
-    # TODO: Allow parallel operation of the JTAG chains
-
-    # Run @ 1MHz
-    clk_period = get_sim_steps(1, "us")
-    master1 = JTAG_Master(dut.i1_tck, dut.i1_tms, dut.i1_tdi, dut.i1_tdo, dut.i1_trst_n, clk_period)
-    master2 = JTAG_Master(dut.i2_tck, dut.i2_tms, dut.i2_tdi, dut.i2_tdo, dut.i2_trst_n, clk_period)
-
-    dut._log.info("Set command to SAMPLEPRELOAD")
-    yield master1.load_ir(master1.SAMPLEPRELOAD)
-    yield master2.load_ir(master2.SAMPLEPRELOAD)
-    
-    dut._log.info("Load data, scan out first sample")
-    yield master1.shift_data([0, 0, 0])
-    dut._log.info("  master1 scan_out: {}".format(master1.result.binstr))
-    assert(master1.result.binstr == "011")
-    yield master2.shift_data([1, 1, 1])
-    dut._log.info("  master2 scan_out: {}".format(master2.result.binstr))
-    assert(master2.result.binstr == "101")
-    
-    dut._log.info("Set command to EXTEST")
-    yield master1.load_ir(master1.EXTEST)
-    yield master2.load_ir(master2.EXTEST)
-
-    dut._log.info("Second scan")
-    yield master1.shift_data([0, 0, 0])
-    dut._log.info("  master1 scan_out: {}".format(master1.result.binstr))
-    assert(master1.result.binstr == "111")
-    yield master2.shift_data([1, 1, 1])
-    dut._log.info("  master2 scan_out: {}".format(master2.result.binstr))
-    assert(master2.result.binstr == "Z01")
diff --git a/sim/ghdl/bench_idcode.sh b/sim/ghdl/bench_idcode.sh
deleted file mode 100755
index e17b656..0000000
--- a/sim/ghdl/bench_idcode.sh
+++ /dev/null
@@ -1,10 +0,0 @@
-#!/bin/sh
-ghdl -a --std=08 ../../rtl/vhdl/c4m_jtag_pkg.vhdl
-ghdl -a --std=08 ../../rtl/vhdl/c4m_jtag_tap_fsm.vhdl
-ghdl -a --std=08 ../../rtl/vhdl/c4m_jtag_irblock.vhdl
-ghdl -a --std=08 ../../rtl/vhdl/c4m_jtag_idblock.vhdl
-ghdl -a --std=08 ../../rtl/vhdl/c4m_jtag_iocell.vhdl
-ghdl -a --std=08 ../../rtl/vhdl/c4m_jtag_ioblock.vhdl
-ghdl -a --std=08 ../../rtl/vhdl/c4m_jtag_tap_controller.vhdl
-ghdl -a --std=08 ../../bench/vhdl/idcode.vhdl
-ghdl -r --std=08 bench_idcode --wave=bench_idcode.ghw
diff --git a/test/cocotb/controller/Makefile b/test/cocotb/controller/Makefile
new file mode 100644
index 0000000..708e50d
--- /dev/null
+++ b/test/cocotb/controller/Makefile
@@ -0,0 +1,22 @@
+PWD=$(realpath .)
+TOPDIR=$(realpath ../../..)
+VHDLDIR=$(TOPDIR)/rtl/vhdl
+VHDL_SOURCES = \
+  $(VHDLDIR)/c4m_jtag_pkg.vhdl \
+  $(VHDLDIR)/c4m_jtag_tap_fsm.vhdl \
+  $(VHDLDIR)/c4m_jtag_irblock.vhdl \
+  $(VHDLDIR)/c4m_jtag_iocell.vhdl \
+  $(VHDLDIR)/c4m_jtag_ioblock.vhdl \
+  $(VHDLDIR)/c4m_jtag_idblock.vhdl \
+  $(VHDLDIR)/c4m_jtag_tap_controller.vhdl
+TOPLEVEL=c4m_jtag_tap_controller
+TOPLEVEL_LANG=vhdl
+MODULE=test
+SIM=ghdl
+GPI_IMPL=vhpi
+SIM_ARGS=--wave=test.ghw
+
+COCOTBDIR=$(shell cocotb-path)
+
+include $(COCOTBDIR)/makefiles/Makefile.inc
+include $(COCOTBDIR)/makefiles/Makefile.sim
diff --git a/test/cocotb/controller/c4m_jtag.py b/test/cocotb/controller/c4m_jtag.py
new file mode 120000
index 0000000..f409baa
--- /dev/null
+++ b/test/cocotb/controller/c4m_jtag.py
@@ -0,0 +1 @@
+../c4m_jtag.py
\ No newline at end of file
diff --git a/test/cocotb/controller/test.py b/test/cocotb/controller/test.py
new file mode 100644
index 0000000..9d66ea8
--- /dev/null
+++ b/test/cocotb/controller/test.py
@@ -0,0 +1,94 @@
+import cocotb
+from cocotb.utils import get_sim_steps
+from cocotb.binary import BinaryValue
+
+from c4m_jtag import JTAG_Master
+
+@cocotb.test()
+def test01_idcode(dut):
+    """
+    Test the IDCODE command
+    """
+
+    # Run @ 1MHz
+    clk_period = get_sim_steps(1, "us")
+    master = JTAG_Master(dut.tck, dut.tms, dut.tdi, dut.tdo, dut.trst_n, clk_period)
+
+    dut._log.info("Trying to get IDCODE...")
+
+    yield master.idcode()
+    result1 = master.result
+    dut._log.info("IDCODE1: {}".format(result1))
+
+    yield master.idcode()
+    result2 = master.result
+    dut._log.info("IDCODE2: {}".format(result2))
+
+    assert(result1 == result2)
+    
+@cocotb.test()
+def test02_bypass(dut):
+    """
+    Test of BYPASS mode
+    """
+
+    # Run @ 1MHz
+    clk_period = get_sim_steps(1, "us")
+    master = JTAG_Master(dut.tck, dut.tms, dut.tdi, dut.tdo, dut.trst_n, clk_period)
+
+    dut._log.info("Loading BYPASS command")
+    yield master.load_ir(master.BYPASS)
+
+    dut._log.info("Sending data")
+
+    data_in = BinaryValue()
+    data_in.binstr = "01001101"
+    yield master.shift_data(data_in)
+
+    dut._log.info("bypass out: {}".format(master.result.binstr))
+    assert(master.result.binstr[:-1] == data_in.binstr[1:])
+
+@cocotb.test()
+def test03_sample(dut):
+    """
+    Test of SAMPLEPRELOAD and EXTEST
+    """
+    data_in = BinaryValue()
+
+    # Run @ 1MHz
+    clk_period = get_sim_steps(1, "us")
+    master = JTAG_Master(dut.tck, dut.tms, dut.tdi, dut.tdo, dut.trst_n, clk_period)
+
+
+    dut._log.info("Load SAMPLEPRELOAD command")
+    yield master.load_ir(master.SAMPLEPRELOAD)
+
+    data_in.binstr = "011"
+    dut._log.info("  preloading data {}".format(data_in.binstr))
+
+    # Set the ios pins
+    dut.core_out = 0
+    dut.core_en = 0
+    dut.pad_in = 1
+    yield master.shift_data(data_in)
+    dut._log.info("  output: {}".format(master.result.binstr))
+    assert(master.result.binstr == "100")
+
+
+    dut._log.info("Load EXTEST command")
+    yield master.load_ir(master.EXTEST)
+
+    data_in.binstr = "100"
+    dut._log.info("  input data {}".format(data_in.binstr))
+    
+    # Set the ios pins
+    dut.core_out = 1
+    dut.core_en = 1
+    dut.pad_in = 0
+    yield master.shift_data(data_in)
+    dut._log.info("  output: {}".format(master.result.binstr))
+    assert(master.result.binstr == "011")
+
+    dut._log.info("Do a capture of the last loaded data")
+    yield master.shift_data([])
+
diff --git a/test/cocotb/dual_parallel/Makefile b/test/cocotb/dual_parallel/Makefile
new file mode 100644
index 0000000..e7da7cd
--- /dev/null
+++ b/test/cocotb/dual_parallel/Makefile
@@ -0,0 +1,23 @@
+PWD=$(realpath .)
+TOPDIR=$(realpath ../../..)
+VHDLDIR=$(TOPDIR)/rtl/vhdl
+VHDL_SOURCES = \
+  $(VHDLDIR)/c4m_jtag_pkg.vhdl \
+  $(VHDLDIR)/c4m_jtag_tap_fsm.vhdl \
+  $(VHDLDIR)/c4m_jtag_irblock.vhdl \
+  $(VHDLDIR)/c4m_jtag_iocell.vhdl \
+  $(VHDLDIR)/c4m_jtag_ioblock.vhdl \
+  $(VHDLDIR)/c4m_jtag_idblock.vhdl \
+  $(VHDLDIR)/c4m_jtag_tap_controller.vhdl \
+  $(PWD)/dual_parallel.vhdl
+TOPLEVEL=dual_parallel
+TOPLEVEL_LANG=vhdl
+MODULE=test
+SIM=ghdl
+GPI_IMPL=vhpi
+SIM_ARGS=--wave=test.ghw
+
+COCOTBDIR=$(shell cocotb-path)
+
+include $(COCOTBDIR)/makefiles/Makefile.inc
+include $(COCOTBDIR)/makefiles/Makefile.sim
diff --git a/test/cocotb/dual_parallel/c4m_jtag.py b/test/cocotb/dual_parallel/c4m_jtag.py
new file mode 120000
index 0000000..f409baa
--- /dev/null
+++ b/test/cocotb/dual_parallel/c4m_jtag.py
@@ -0,0 +1 @@
+../c4m_jtag.py
\ No newline at end of file
diff --git a/test/cocotb/dual_parallel/dual_parallel.vhdl b/test/cocotb/dual_parallel/dual_parallel.vhdl
new file mode 100644
index 0000000..989d15a
--- /dev/null
+++ b/test/cocotb/dual_parallel/dual_parallel.vhdl
@@ -0,0 +1,82 @@
+-- Top cell with two instantiations of the tap_controller with parallel scan chains
+
+library ieee;
+use ieee.std_logic_1164.ALL;
+
+use work.c4m_jtag.ALL;
+
+entity dual_parallel is
+  port (
+    -- Instance 1
+    -- ==========
+    -- JTAG
+    I1_TCK:     in std_logic;
+    I1_TMS:     in std_logic;
+    I1_TDI:     in std_logic;
+    I1_TDO:     out std_logic;
+    I1_TRST_N:  in std_logic;
+
+    -- Instance 2
+    -- ==========
+    -- JTAG
+    I2_TCK:     in std_logic;
+    I2_TMS:     in std_logic;
+    I2_TDI:     in std_logic;
+    I2_TDO:     out std_logic;
+    I2_TRST_N:  in std_logic
+  );
+end dual_parallel;
+
+architecture rtl of dual_parallel is
+  signal I1_PAD_IN:     std_logic;
+  signal I1_PAD_EN:     std_logic;
+  signal I1_PAD_OUT:    std_logic;
+  signal I2_PAD_IN:     std_logic;
+  signal I2_PAD_EN:     std_logic;
+  signal I2_PAD_OUT:    std_logic;
+begin
+  CTRL1: c4m_jtag_tap_controller
+    port map (
+      TCK => I1_TCK,
+      TMS => I1_TMS,
+      TDI => I1_TDI,
+      TDO => I1_TDO,
+      TRST_N => I1_TRST_N,
+      RESET => open,
+      DRCAPTURE => open,
+      DRSHIFT => open,
+      DRUPDATE => open,
+      IR => open,
+      CORE_IN => open,
+      CORE_EN => "1",
+      CORE_OUT => "1",
+      PAD_IN(0) => I1_PAD_IN,
+      PAD_EN(0) => I1_PAD_EN,
+      PAD_OUT(0) => I1_PAD_OUT
+    );
+
+  CTRL2: c4m_jtag_tap_controller
+    port map (
+      TCK => I2_TCK,
+      TMS => I2_TMS,
+      TDI => I2_TDI,
+      TDO => I2_TDO,
+      TRST_N => I2_TRST_N,
+      RESET => open,
+      DRCAPTURE => open,
+      DRSHIFT => open,
+      DRUPDATE => open,
+      IR => open,
+      CORE_IN => open,
+      CORE_EN => "1",
+      CORE_OUT => "0",
+      PAD_IN(0) => I2_PAD_IN,
+      PAD_EN(0) => I2_PAD_EN,
+      PAD_OUT(0) => I2_PAD_OUT
+    );
+
+  I1_PAD_IN <= I2_PAD_OUT when I2_PAD_EN = '1' else
+               'Z';
+  I2_PAD_IN <= I1_PAD_OUT when I1_PAD_EN = '1' else
+               'Z';
+end rtl;
diff --git a/test/cocotb/dual_parallel/test.py b/test/cocotb/dual_parallel/test.py
new file mode 100644
index 0000000..47cf306
--- /dev/null
+++ b/test/cocotb/dual_parallel/test.py
@@ -0,0 +1,41 @@
+import cocotb
+from cocotb.utils import get_sim_steps
+
+from c4m_jtag import JTAG_Master
+
+@cocotb.test()
+def test01_dual(dut):
+    """
+    Test the IDCODE command
+    """
+
+    # TODO: Allow parallel operation of the JTAG chains
+
+    # Run @ 1MHz
+    clk_period = get_sim_steps(1, "us")
+    master1 = JTAG_Master(dut.i1_tck, dut.i1_tms, dut.i1_tdi, dut.i1_tdo, dut.i1_trst_n, clk_period)
+    master2 = JTAG_Master(dut.i2_tck, dut.i2_tms, dut.i2_tdi, dut.i2_tdo, dut.i2_trst_n, clk_period)
+
+    dut._log.info("Set command to SAMPLEPRELOAD")
+    yield master1.load_ir(master1.SAMPLEPRELOAD)
+    yield master2.load_ir(master2.SAMPLEPRELOAD)
+    
+    dut._log.info("Load data, scan out first sample")
+    yield master1.shift_data([0, 0, 0])
+    dut._log.info("  master1 scan_out: {}".format(master1.result.binstr))
+    assert(master1.result.binstr == "011")
+    yield master2.shift_data([1, 1, 1])
+    dut._log.info("  master2 scan_out: {}".format(master2.result.binstr))
+    assert(master2.result.binstr == "101")
+    
+    dut._log.info("Set command to EXTEST")
+    yield master1.load_ir(master1.EXTEST)
+    yield master2.load_ir(master2.EXTEST)
+
+    dut._log.info("Second scan")
+    yield master1.shift_data([0, 0, 0])
+    dut._log.info("  master1 scan_out: {}".format(master1.result.binstr))
+    assert(master1.result.binstr == "111")
+    yield master2.shift_data([1, 1, 1])
+    dut._log.info("  master2 scan_out: {}".format(master2.result.binstr))
+    assert(master2.result.binstr == "Z01")
diff --git a/test/ghdl/idcode/bench_idcode.sh b/test/ghdl/idcode/bench_idcode.sh
new file mode 100755
index 0000000..e17b656
--- /dev/null
+++ b/test/ghdl/idcode/bench_idcode.sh
@@ -0,0 +1,10 @@
+#!/bin/sh
+ghdl -a --std=08 ../../rtl/vhdl/c4m_jtag_pkg.vhdl
+ghdl -a --std=08 ../../rtl/vhdl/c4m_jtag_tap_fsm.vhdl
+ghdl -a --std=08 ../../rtl/vhdl/c4m_jtag_irblock.vhdl
+ghdl -a --std=08 ../../rtl/vhdl/c4m_jtag_idblock.vhdl
+ghdl -a --std=08 ../../rtl/vhdl/c4m_jtag_iocell.vhdl
+ghdl -a --std=08 ../../rtl/vhdl/c4m_jtag_ioblock.vhdl
+ghdl -a --std=08 ../../rtl/vhdl/c4m_jtag_tap_controller.vhdl
+ghdl -a --std=08 ../../bench/vhdl/idcode.vhdl
+ghdl -r --std=08 bench_idcode --wave=bench_idcode.ghw
diff --git a/test/rtl/vhdl/idcode.vhdl b/test/rtl/vhdl/idcode.vhdl
new file mode 100644
index 0000000..550df98
--- /dev/null
+++ b/test/rtl/vhdl/idcode.vhdl
@@ -0,0 +1,102 @@
+-- reset JTAG interface and then IDCODE should be shifted out
+
+library ieee;
+use ieee.std_logic_1164.ALL;
+
+use work.c4m_jtag.ALL;
+
+entity bench_idcode is
+end bench_idcode;
+
+architecture rtl of bench_idcode is
+  signal TCK:   std_logic;
+  signal TMS:   std_logic;
+  signal TDI:   std_logic;
+  signal TDO:   std_logic;
+  signal TRST_N: std_logic;
+
+  constant CLK_PERIOD:  time := 10 ns;
+
+  procedure ClkCycle(
+    signal CLK: out std_logic;
+    CLK_PERIOD: time
+  ) is
+  begin
+    CLK <= '0';
+    wait for CLK_PERIOD/4;
+    CLK <= '1';
+    wait for CLK_PERIOD/2;
+    CLK <= '0';
+    wait for CLK_PERIOD/4;
+  end ClkCycle;
+
+  procedure ClkCycles(
+    N:  integer;
+    signal CLK: out std_logic;
+    CLK_PERIOD: time
+  ) is
+  begin
+    for i in 1 to N loop
+      ClkCycle(CLK, CLK_PERIOD);
+    end loop;
+  end ClkCycles;
+begin
+  JTAG_BLOCK: c4m_jtag_tap_controller
+    -- Use default values
+    port map (
+      TCK => TCK,
+      TMS => TMS,
+      TDI => TDI,
+      TDO => TDO,
+      TRST_N => TRST_N,
+      RESET => open,
+      DRCAPTURE => open,
+      DRSHIFT => open,
+      DRUPDATE => open,
+      IR => open,
+      CORE_OUT => "0",
+      CORE_IN => open,
+      CORE_EN => "0",
+      PAD_OUT => open,
+      PAD_IN => "0",
+      PAD_EN => open
+    );
+
+  SIM: process
+  begin
+    -- Reset
+    TCK <= '0';
+    TMS <= '1';
+    TDI <= '0';
+    TRST_N <= '0';
+    wait for 10*CLK_PERIOD;
+
+    TRST_N <= '1';
+    wait for CLK_PERIOD;
+
+    -- Enter RunTestIdle
+    TMS <= '0';
+    ClkCycle(TCK, CLK_PERIOD);
+    -- Enter SelectDRScan
+    TMS <= '1';
+    ClkCycle(TCK, CLK_PERIOD);
+    -- Enter Capture
+    TMS <= '0';
+    ClkCycle(TCK, CLK_PERIOD);
+    -- Enter Shift, run for 35 CLK cycles
+    TMS <= '0';
+    ClkCycles(35, TCK, CLK_PERIOD);
+    -- Enter Exit1
+    TMS <= '1';
+    ClkCycle(TCK, CLK_PERIOD);
+    -- Enter Update
+    TMS <= '1';
+    ClkCycle(TCK, CLK_PERIOD);
+    -- To TestLogicReset
+    TMS <= '1';
+    ClkCycles(4, TCK, CLK_PERIOD);
+
+    -- end simulation
+    wait;
+  end process;
+end rtl;
diff --git a/test/rtl/vhdl/sampleshift.vhdl b/test/rtl/vhdl/sampleshift.vhdl
new file mode 100644
index 0000000..62ec28b
--- /dev/null
+++ b/test/rtl/vhdl/sampleshift.vhdl
@@ -0,0 +1,50 @@
+-- Test JTAG in the following way:
+--  * reset JTAG interface
+--  * load samplepreload command
+--  * shift in/out sampled inputs + wanted outputs
+--  * load extest command
+--  * execute
+
+
+library ieee;
+use ieee.std_logic_1164.ALL;
+
+use work.c4m_jtag.ALL;
+
+entity bench_sampleshift is
+end bench_sampleshift;
+
+architecture rtl of bench_sampleshift is
+  signal TCK:   std_logic;
+  signal TMS:   std_logic;
+  signal TDI:   std_logic;
+  signal TDO:   std_logic;
+  signal TRST_N: std_logic;
+  
+  constant CLK_PERIOD:  time := 10 ns;
+
+  procedure ClkCycle(
+    signal CLK: out std_logic;
+    CLK_PERIOD: time
+  ) is
+  begin
+    CLK <= '0';
+    wait for CLK_PERIOD/4;
+    CLK <= '1';
+    wait for CLK_PERIOD/2;
+    CLK <= '0';
+    wait for CLK_PERIOD/4;
+  end ClkCycle;
+
+  procedure ClkCycles(
+    N:  integer;
+    signal CLK: out std_logic;
+    CLK_PERIOD: time
+  ) is
+  begin
+    for i in 1 to N loop
+      ClkCycle(CLK, CLK_PERIOD);
+    end loop;
+  end ClkCycles;
+  
+  procedure LoadIR(