"""Initialize CPU and setup clock"""
 
     clk_steps = get_sim_steps(clk_period, "ns")
-    cocotb.fork(Clock(dut.clk_from_pad, clk_steps).start())
+    cocotb.fork(Clock(dut.sys_clk, clk_steps).start())
 
-    dut.rst_from_pad <= 1
-    dut.clk_from_pad <= 0
+    dut.sys_rst <= 1
+    dut.sys_clk <= 0
     if run:
         yield Timer(int(10.5*clk_steps))
-        dut.rst_from_pad <= 0
+        dut.sys_rst <= 0
         yield Timer(int(5*clk_steps))
 
 def setup_jtag(dut, *, tck_period):
     # Make this a generator
     if False:
         yield Timer(0)
-    return JTAG_Master(dut.tck_from_pad, dut.tms_from_pad,
-                       dut.tdi_from_pad, dut.tdo_to_pad,
+    return JTAG_Master(dut.jtag_tck, dut.jtag_tms,
+                       dut.jtag_tdi, dut.jtag_tdo,
                        clk_period=tck_period,
                        ir_width=4)
 
 
     dut._log.info("IDCODE test completed")
 
+from itertools import chain
+
+import cocotb
+from cocotb.clock import Clock
+from cocotb.triggers import Timer
+from cocotb.utils import get_sim_steps
+from cocotb.binary import BinaryValue
+
+from c4m.nmigen.jtag.tap import IOType
+from c4m.cocotb.jtag.c4m_jtag import JTAG_Master
+from c4m.cocotb.jtag.c4m_jtag_svfcocotb import SVF_Executor
+
+from soc.config.pinouts import get_pinspecs
+from soc.debug.jtag import Pins
+
+
+@cocotb.test()
+def wishbone_basic(dut):
+    """
+    Test of an added Wishbone interface
+    """
+    clk_period = 100 # 10MHz
+    tck_period = 3000 # 0.3MHz
+
+    data_in = BinaryValue()
+    # these have to match with soc.debug.jtag.JTAG ircodes
+    cmd_MEMADDRESS = BinaryValue("0101")    # 5
+    cmd_MEMREAD = BinaryValue("0110")       # 6
+    cmd_MEMREADWRITE = BinaryValue("0111")  # 7
+
+    info = "Running Wishbone basic test"
+    yield from setup_sim(dut, clk_period=clk_period, run=True)
+    master = yield from setup_jtag(wrap, tck_period = tck_period)
+
+    # Load the memory address
+    yield master.load_ir(cmd_MEMADDRESS)
+    dut._log.info("Loading address")
+
+    data_in.binstr = "000000000000000000000000000001"
+    dut._log.info("  input: {}".format(data_in.binstr))
+    yield master.shift_data(data_in)
+    dut._log.info("  output: {}".format(master.result.binstr))
+
+    # Do write
+    yield master.load_ir(cmd_MEMREADWRITE)
+    dut._log.info("Writing memory")
+
+    data_in.binstr = "01010101" * 8
+    dut._log.info("  input: {}".format(data_in.binstr))
+    yield master.shift_data(data_in)
+    dut._log.info("  output: {}".format(master.result.binstr))
+
+    data_in.binstr = "10101010" * 8
+    dut._log.info("  input: {}".format(data_in.binstr))
+    yield master.shift_data(data_in)
+    dut._log.info("  output: {}".format(master.result.binstr))
+
+    # Load the memory address
+    yield master.load_ir(cmd_MEMADDRESS)
+    dut._log.info("Loading address")
+
+    data_in.binstr = "000000000000000000000000000001"
+    dut._log.info("  input: {}".format(data_in.binstr))
+    yield master.shift_data(data_in)
+    dut._log.info("  output: {}".format(master.result.binstr))
+    assert master.result.binstr == "000000000000000000000000000000"
+
+    # Do read and write
+    yield master.load_ir(cmd_MEMREADWRITE)
+    dut._log.info("Reading and writing memory")
+
+    data_in.binstr = "10101010" * 4
+    dut._log.info("  input: {}".format(data_in.binstr))
+    yield master.shift_data(data_in)
+    dut._log.info("  output: {}".format(master.result.binstr))
+    assert master.result.binstr == "01010101" * 4
+
+    data_in.binstr = "01010101" * 4
+    dut._log.info("  input: {}".format(data_in.binstr))
+    yield master.shift_data(data_in)
+    dut._log.info("  output: {}".format(master.result.binstr))
+    assert master.result.binstr == "10101010" * 4
+
+    # Load the memory address
+    yield master.load_ir(cmd_MEMADDRESS)
+    dut._log.info("Loading address")
+
+    data_in.binstr = "000000000000000000000000000001"
+    dut._log.info("  input: {}".format(data_in.binstr))
+    yield master.shift_data(data_in)
+    dut._log.info("  output: {}".format(master.result.binstr))
+    assert master.result.binstr == "000000000000000000000000000010"
+
+    # Do read
+    yield master.load_ir(cmd_MEMREAD)
+    dut._log.info("Reading memory")
+    data_in.binstr = "00000000" * 4
+
+    dut._log.info("  input: {}".format(data_in.binstr))
+    yield master.shift_data(data_in)
+    dut._log.info("  output: {}".format(master.result.binstr))
+    assert master.result.binstr == "10101010" * 4
+
+    dut._log.info("  input: {}".format(data_in.binstr))
+    yield master.shift_data(data_in)
+    dut._log.info("  output: {}".format(master.result.binstr))
+    assert master.result.binstr == "01010101" * 4
+
+    # Load the memory address
+    yield master.load_ir(cmd_MEMADDRESS) # MEMADDR
+    dut._log.info("Loading address")
+
+    data_in.binstr = "0000000000000000000000000000001"
+    dut._log.info("  input: {}".format(data_in.binstr))
+    yield master.shift_data(data_in)
+    dut._log.info("  output: {}".format(master.result.binstr))
+    assert master.result.binstr == "000000000000000000000000000010"
+
+    # Do read
+    yield master.load_ir(cmd_MEMREAD) # MEMREAD
+    dut._log.info("Reading memory")
+    data_in.binstr = "00000000" * 4
+
+    dut._log.info("  input: {}".format(data_in.binstr))
+    yield master.shift_data(data_in)
+    dut._log.info("  output: {}".format(master.result.binstr))
+    assert master.result.binstr == "10101010" * 4
+
+    dut._log.info("  input: {}".format(data_in.binstr))
+    yield master.shift_data(data_in)
+    dut._log.info("  output: {}".format(master.result.binstr))
+    assert master.result.binstr == "01010101" * 4
+
+    dut._log.info("{!r}".format(wbmem))
+
+
+# demo / debug how to get boundary scan names. run "python3 test.py"
+if __name__ == '__main__':
+    pinouts = get_jtag_boundary()
+    for pin in pinouts:
+        # example: ('eint', '2', <IOType.In: 1>, 'eint_2', 125)
+        print (pin)