Added DMI and WB code from test_jtag_tap, basic pad/core toggling not working yet