diff --git a/drivers/jtaglib_cpu16.c b/drivers/jtaglib_cpu16.c
index 293fae8..cbebeba 100644
--- a/drivers/jtaglib_cpu16.c
+++ b/drivers/jtaglib_cpu16.c
@@ -3,6 +3,9 @@
 #include "jtaglib_defs.h"
 #include "output.h"
 
+// FIXME:
+// * stepping works only once. or maybe sometimes never.
+
 /* Set target CPU JTAG state machine into the instruction fetch state
  * return: 1 - instruction fetch was set
  *         0 - otherwise
@@ -44,7 +47,7 @@ static void jlf16_halt_cpu(struct jtdev *p)
 	/* Send JMP $ instruction to keep CPU from changing the state */
 	jtag_ir_shift(p, IR_DATA_16BIT);
 	jtag_dr_shift_16(p, 0x3FFF);
-	//jtag_tclk_set(p); // TODO: ???
+	jtag_tclk_set(p); // TODO: ???
 	jtag_tclk_clr(p);
 
 	/* Set JTAG_HALT bit */
@@ -221,9 +224,12 @@ static void jlf16_read_mem_quick(struct jtdev *p, address_t address,
 			 unsigned int length, uint16_t *data)
 {	// SLAU320AJ name: ReadMemQuick
 	unsigned int index;
+	address_t pc_bak;
+
+	pc_bak = jtag_read_reg(p, 0);
 
 	/* Initialize reading: */
-	jtag_write_reg(p, 0,address-4);
+	jtag_write_reg(p, 0, address-4);
 	jlf16_halt_cpu(p);
 	jtag_tclk_clr(p);
 
@@ -234,14 +240,15 @@ static void jlf16_read_mem_quick(struct jtdev *p, address_t address,
 
 	for (index = 0; index < length; index++) {
 		jtag_tclk_set(p);
-		//jtag_tclk_clr(p); // TODO: ???
+		jtag_tclk_clr(p); // TODO: ???
 		/* shift out the data from the target */
 		data[index] = jtag_dr_shift_16(p, 0x0000);
-		jtag_tclk_clr(p); // TODO: ???
+		//jtag_tclk_clr(p); // TODO: ???
 	}
 
 	jtag_tclk_set(p);
 	jlf16_release_cpu(p);
+	jtag_write_reg(p, 0, pc_bak);
 }
 
 /* Writes one byte/word at a given address
@@ -291,11 +298,12 @@ static void jlf16_write_mem_quick(struct jtdev *p, address_t address,
 	jtag_tclk_clr(p);
 	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
 
-	for (index = 0; index < length; index++) {
-		/* Set RW to write */
-		jtag_dr_shift_16(p, 0x2408);
-		jtag_ir_shift(p, IR_DATA_QUICK);
+	/* Set RW to write */
+	jtag_dr_shift_16(p, 0x2408);
+	jtag_ir_shift(p, IR_DATA_QUICK);
+	// TODO: ^ in start of loop?
 
+	for (index = 0; index < length; index++) {
 		/* Write data */
 		jtag_dr_shift_16(p, data[index]);
 
@@ -325,12 +333,12 @@ static unsigned int jlf16_execute_puc(struct jtdev *p)
 	jtag_tclk_clr(p);
 	jtag_tclk_set(p);
 	jtag_tclk_clr(p);
-	//jtag_tclk_set(p); // TODO: ???
+	jtag_tclk_set(p); // TODO: ???
 
 	/* Read jtag id */
 	jtag_id = jtag_ir_shift(p, IR_ADDR_CAPTURE);
 
-	jtag_tclk_set(p); // TODO: ???
+	//jtag_tclk_set(p); // TODO: ???
 
 	/* Disable watchdog on target device */
 	jtag_write_mem(p, 16, 0x0120, 0x5A80); // FIXME
@@ -607,9 +615,9 @@ static address_t jlf16_read_reg(struct jtdev *p, int reg)
 	 */
 	jtag_dr_shift_16(p, 0x4082 | (((unsigned int)reg << 8) & 0x0f00) );
 	jtag_tclk_clr(p);
-	jtag_ir_shift(p, IR_DATA_CAPTURE); // TODO: ???
+	//jtag_ir_shift(p, IR_DATA_CAPTURE); // TODO: ???
 	jtag_tclk_set(p);
-	jtag_ir_shift(p, IR_DATA_16BIT); // TODO: ???
+	//jtag_ir_shift(p, IR_DATA_16BIT); // TODO: ???
 	jtag_dr_shift_16(p, 0x01fe);
 	jtag_tclk_clr(p);
 	jtag_tclk_set(p);
@@ -672,8 +680,8 @@ static void jlf16_write_reg(struct jtdev *p, int reg, address_t value)
 	jtag_tclk_set(p);
 
 	// TODO: ???
-	jtag_ir_shift(p, IR_ADDR_CAPTURE);
-	jtag_tclk_clr(p);
+	//jtag_ir_shift(p, IR_ADDR_CAPTURE);
+	//jtag_tclk_clr(p);
 
 	/* JTAG controls RW & BYTE */
 	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);