jtaglib: properly handle USB ram (cannot use quick memory access routines here)

In order to support CPUX and Xv2 devices, some refactoring is needed:

* split jtaglib into files concerning low-level JTAG/SBW physical layer
  stuff, and a file containing the actual debug routines. the latter is
  implemented in a separate file per CPU type (CPUX and Xv2 currently empty)
* use a lookup table dependig on the JTAG ID or CPU type to select which
  function to use
* move the 'struct device' dependency in pif and mehfet to jtdev, as it
  is needed everywhere, especially for device identification

In a next step, the following actions can be performed:

* Unify the read and write callbacks in pif and mehfet (used in readmem
  and writemem of struct device) to remove the redundancy
* Unify the init_device function in pif and mehfet to properly detect
  the device type, as done in v3hil. The device base type does query
  this data independenly, however, which feels unwanted.

Makefile

@@ -21,6 +21,8 @@ INSTALL = /usr/bin/install
 PREFIX ?= /usr/local
 LDFLAGS ?= -s
 
+-include config.mk
+
 BINDIR = ${PREFIX}/bin/
 MANDIR = ${PREFIX}/share/man/man1
 LIBDIR = ${PREFIX}/lib/
@@ -195,6 +197,9 @@ OBJ=\
     drivers/jtdev_bus_pirate.o \
     drivers/jtdev_gpio.o \
     drivers/jtaglib.o \
+    drivers/jtaglib_cpu16.o \
+    drivers/jtaglib_cpux.o \
+    drivers/jtaglib_cpuxv2.o \
     drivers/mehfet_proto.o \
     drivers/mehfet.o \
     drivers/pif.o \

drivers/jtaglib.h

@@ -34,16 +34,56 @@
 #ifndef JTAGLIB_H_
 #define JTAGLIB_H_
 
+#include <stdbool.h>
 #include <stdint.h>
 
 #include "device.h"
 #include "jtdev.h"
 #include "util.h"
 
-/* Flash erasing modes */
-#define JTAG_ERASE_MASS 0xA506
-#define JTAG_ERASE_MAIN 0xA504
-#define JTAG_ERASE_SGMT 0xA502
+/* Colleciton of functions that need different implementation for different
+ * CPU types (that is, 16-bit vs CPUX vs Xv2) */
+struct jtaglib_funcs {
+	unsigned int (*jlf_get_device)(struct jtdev *p);
+
+	uint16_t (*jlf_read_mem)(struct jtdev *p, unsigned int format, address_t address);
+	void (*jlf_read_mem_quick)(struct jtdev *p, address_t start_address,
+			unsigned int word_count, uint16_t* data);
+	void (*jlf_write_mem)(struct jtdev *p, unsigned int format,
+			address_t address, uint16_t data);
+	void (*jlf_write_mem_quick)(struct jtdev *p, address_t start_address,
+			unsigned int word_count, const uint16_t* data);
+
+	unsigned int (*jlf_execute_puc)(struct jtdev *p);
+	void (*jlf_release_device)(struct jtdev *p, address_t address);
+
+	int (*jlf_verify_mem)(struct jtdev *p, address_t start_address,
+			unsigned int length, const uint16_t *data);
+	int (*jlf_erase_check)(struct jtdev *p, address_t start_address,
+			unsigned int length);
+
+	void (*jlf_write_flash)(struct jtdev *p, address_t start_address,
+			unsigned int word_count, const uint16_t *data);
+	void (*jlf_erase_flash)(struct jtdev *p, unsigned int erase_mode,
+			address_t erase_address);
+
+	address_t (*jlf_read_reg)(struct jtdev *p, int reg);
+	void (*jlf_write_reg)(struct jtdev *p, int reg, address_t value);
+	void (*jlf_single_step)(struct jtdev *p);
+	unsigned int (*jlf_set_breakpoint)(struct jtdev *p, int bp_num,
+			address_t bp_addr);
+	unsigned int (*jlf_cpu_state)(struct jtdev *p);
+	int (*jlf_get_config_fuses)(struct jtdev *p);
+
+	void (*jlf_context_save)(struct jtdev *p, bool after_puc);
+	void (*jlf_context_restore)(struct jtdev *p);
+	void (*jlf_regs_update)(struct jtdev *p);
+	void (*jlf_regs_flush)(struct jtdev *p);
+};
+
+extern const struct jtaglib_funcs jlf_cpu16;
+extern const struct jtaglib_funcs jlf_cpux;
+extern const struct jtaglib_funcs jlf_cpuxv2;
 
 /* Take target device under JTAG control. */
 unsigned int jtag_init(struct jtdev *p);
@@ -118,14 +158,35 @@ unsigned int jtag_set_breakpoint(struct jtdev *p,
 unsigned int jtag_cpu_state(struct jtdev *p);
 int jtag_get_config_fuses(struct jtdev *p);
 
+void jtag_dev_context_save(struct jtdev *p, bool after_puc);
+void jtag_dev_context_restore(struct jtdev *p);
+void jtag_dev_regs_update(struct jtdev *p);
+void jtag_dev_regs_flush(struct jtdev *p);
+
 /* Default low-level JTAG routines for jtdev implementations that don't have
  * their own implementations of these routines */
 uint8_t jtag_default_ir_shift(struct jtdev *p, uint8_t ir);
 uint8_t jtag_default_dr_shift_8(struct jtdev *p, uint8_t dr);
 uint16_t jtag_default_dr_shift_16(struct jtdev *p, uint16_t dr);
+uint32_t jtag_default_dr_shift_20(struct jtdev *p, uint32_t dr);
 void jtag_default_tms_sequence(struct jtdev *p, int bits, unsigned int value);
 void jtag_default_init_dap(struct jtdev *p);
 
-int jtag_refresh_bps(const char *driver, device_t dev, struct jtdev *p);
+void jtag_dev_default_context_save(struct jtdev *p, bool after_puc);
+void jtag_dev_default_context_restore(struct jtdev *p);
+void jtag_dev_default_regs_update(struct jtdev *p);
+void jtag_dev_default_regs_flush(struct jtdev *p);
+
+int jtag_refresh_bps(device_t dev, struct jtdev *p);
+
+int jtag_dev_readmem(device_t dev_base, address_t addr, uint8_t *mem, address_t len);
+int jtag_dev_writemem(device_t dev_base, address_t addr, const uint8_t *mem, address_t len);
+int jtag_dev_getregs(device_t dev_base, address_t *regs);
+int jtag_dev_setregs(device_t dev_base, const address_t *regs);
+int jtag_dev_ctl(device_t dev_base, device_ctl_t type);
+device_status_t jtag_dev_poll(device_t dev_base);
+int jtag_dev_erase(device_t dev_base, device_erase_type_t, address_t addr);
+int jtag_dev_getconfigfuses(device_t dev_base);
+int jtag_dev_init(struct jtdev *p);
 
 #endif

drivers/jtaglib_cpu16.c

@@ -0,0 +1,883 @@
+
+#include "jtaglib.h"
+#include "jtaglib_defs.h"
+#include "output.h"
+
+#ifdef DEBUG_JTAGLIB
+#define dbg_printc(fmt, ...) printc_dbg("jlf16: %s:%d " fmt, __func__, __LINE__, ##__VA_ARGS__)
+#else
+#define dbg_printc(fmt, ...) do{}while(0)
+#endif
+
+/* Set target CPU JTAG state machine into the instruction fetch state
+ * return: 1 - instruction fetch was set
+ *         0 - otherwise
+ */
+static int jlf16_set_instruction_fetch(struct jtdev *p)
+{	// SLAU320AJ name: SetInstrFetch / SyncJtag?
+	unsigned int loop_counter;
+
+	// SyncJtag: has CTLR_SIG_16BIT=0x2401 here
+
+	jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
+	/* Wait until CPU is in instruction fetch state
+	 * timeout after limited attempts
+	 */
+	for (loop_counter = 50; loop_counter > 0; loop_counter--) {
+		if ((jtag_dr_shift_16(p, 0x0000) & 0x0080) == 0x0080)
+			return 1;
+
+		jtag_tclk_clr(p); /* The TCLK pulse befor jtag_dr_shift_16 leads to   */
+		jtag_tclk_set(p); /* problems at MEM_QUICK_READ, it's from SLAU265 */
+	}
+
+	printc_err("jlf16_set_instruction_fetch: failed\n");
+	p->failed = 1;
+
+	return 0;
+}
+
+/* Set the CPU into a controlled stop state */
+static void jlf16_halt_cpu(struct jtdev *p)
+{	// SLAU320AJ name: HaltCPU
+	/* Set CPU into instruction fetch mode */
+	dbg_printc("halt cpu\n");
+
+	jlf16_set_instruction_fetch(p);
+
+	/* Set device into JTAG mode + read */
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+	jtag_dr_shift_16(p, 0x2401);
+
+	/* 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_clr(p);
+
+	/* Set JTAG_HALT bit */
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+	jtag_dr_shift_16(p, 0x2409);
+	jtag_tclk_set(p);
+}
+
+/* Release the target CPU from the controlled stop state */
+static void jlf16_release_cpu(struct jtdev *p)
+{	// SLAU320AJ name: ReleaseCPU
+	dbg_printc("release cpu\n");
+
+	jtag_tclk_clr(p);
+
+	/* clear the HALT_JTAG bit */
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+	jtag_dr_shift_16(p, 0x2401);
+	jtag_ir_shift(p, IR_ADDR_CAPTURE);
+	jtag_tclk_set(p);
+}
+
+/* Compares the computed PSA (Pseudo Signature Analysis) value to the PSA
+ * value shifted out from the target device. It is used for very fast data
+ * block write or erasure verification.
+ * start_address: start of data
+ * length       : number of data
+ * data         : pointer to data, 0 for erase check
+ * RETURN       : 1 - comparison was successful
+ *                0 - otherwise
+ */
+static int jlf16_verify_mem(struct jtdev *p,
+			   unsigned int start_address,
+			   unsigned int length,
+			   const uint16_t *data)
+{	// SLAU320AJ name: VerifyMem/VerifyPSA
+	unsigned int psa_value;
+	unsigned int index;
+
+	/* Polynom value for PSA calculation */
+	unsigned int polynom = 0x0805;
+	/* Start value for PSA calculation */
+	unsigned int psa_crc = start_address-2;
+
+	dbg_printc("verify: %04x..%04x\n", start_address, start_address+length*2);
+
+	jtag_execute_puc(p);
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+	jtag_dr_shift_16(p, 0x2401);
+	jlf16_set_instruction_fetch(p);
+	jtag_ir_shift(p, IR_DATA_16BIT);
+	jtag_dr_shift_16(p, 0x4030);
+	jtag_tclk_set(p);
+	jtag_tclk_clr(p);
+	jtag_dr_shift_16(p, start_address-2);
+	jtag_tclk_set(p);
+	jtag_tclk_clr(p);
+	jtag_tclk_set(p);
+	jtag_tclk_clr(p);
+	jtag_tclk_set(p);
+	jtag_tclk_clr(p);
+	jtag_ir_shift(p, IR_ADDR_CAPTURE);
+	jtag_dr_shift_16(p, 0x0000);
+	jtag_ir_shift(p, IR_DATA_PSA);
+
+	for (index = 0; index < length; index++) {
+		/* Calculate the PSA value */
+		if ((psa_crc & 0x8000) == 0x8000) {
+			psa_crc  ^= polynom;
+			psa_crc <<= 1;
+			psa_crc  |= 0x0001;
+		} else
+			psa_crc <<= 1;
+
+		if (data == 0)
+			/* use erase check mask */
+			psa_crc ^= 0xFFFF;
+		else
+			/* use data */
+			psa_crc ^= data[index];
+
+		/* Clock through the PSA */
+		jtag_tclk_set(p);
+
+		/* Go through DR path without shifting data in/out */
+		jtag_tms_sequence(p, 6, 0x19); /* TMS=1 0 0 1 1 0 ; 6 clocks */
+
+		jtag_tclk_clr(p);
+	}
+
+	/* Read out the PSA value */
+	jtag_ir_shift(p, IR_SHIFT_OUT_PSA);
+	psa_value = jtag_dr_shift_16(p, 0x0000);
+	jtag_tclk_set(p);
+
+	return (psa_value == psa_crc) ? 1 : 0;
+}
+
+/* ------------------------------------------------------------------------- */
+
+static int jlf16_erase_check(struct jtdev *p, unsigned int start_address,
+			   unsigned int length)
+{
+	return jlf16_verify_mem(p, start_address, length, NULL);
+}
+
+static unsigned int jlf16_get_device(struct jtdev *p)
+{	// SLAU320AJ name: GetDevice
+	unsigned int jtag_id = 0;
+	unsigned int loop_counter;
+
+	/* Set device into JTAG mode + read */
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+	jtag_dr_shift_16(p, 0x2401);
+
+	/* Wait until CPU is synchronized,
+	 * timeout after a limited number of attempts
+	 */
+	jtag_id = jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
+	for ( loop_counter = 50; loop_counter > 0; loop_counter--) {
+		if ( (jtag_dr_shift_16(p, 0x0000) & 0x0200) == 0x0200 ) {
+			break;
+		}
+	}
+
+	dbg_printc("get device: jtag id=%02x\n", jtag_id);
+
+	if (loop_counter == 0) {
+		printc_err("jlf16_get_device: timed out\n");
+		p->failed = 1;
+		/* timeout reached */
+		return 0;
+	}
+
+	return jtag_id;
+}
+
+/* Reads one byte/word from a given address
+ * format : 8-byte, 16-word
+ * address: address of memory
+ * return : content of memory
+ */
+static uint16_t jlf16_read_mem(struct jtdev *p, unsigned int format, address_t address)
+{	// SLAU320AJ name: ReadMem
+	uint16_t content;
+
+	dbg_printc("%dbit %04x\n", format, address);
+
+	jlf16_halt_cpu(p);
+	jtag_tclk_clr(p);
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+	if (format == 16) {
+		/* set word read */
+		jtag_dr_shift_16(p, 0x2409);
+	} else {
+		/* set byte read */
+		jtag_dr_shift_16(p, 0x2419);
+	}
+	/* set address */
+	jtag_ir_shift(p, IR_ADDR_16BIT);
+	jtag_dr_shift_16(p, address);
+	jtag_ir_shift(p, IR_DATA_TO_ADDR);
+	jtag_tclk_set(p);
+	jtag_tclk_clr(p);
+
+	/* shift out 16 bits */
+	content = jtag_dr_shift_16(p, 0x0000);
+	jtag_tclk_set(p); /* is also the first instruction in jtag_release_cpu() */
+	jlf16_release_cpu(p);
+	if (format == 8)
+		content &= 0x00ff;
+
+	dbg_printc("%dbit %04x -> %04x\n", format, address, content);
+
+	return content;
+}
+
+/* Reads an array of words from target memory
+ * address: address to read from
+ * length : number of word to read
+ * data   : memory to write to
+ */
+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;
+
+	dbg_printc("%04x..%04x\n", address, address+length*2);
+
+	pc_bak = jtag_read_reg(p, 0);
+
+	/* Initialize reading: */
+	jtag_write_reg(p, 0, address-4);
+	jlf16_halt_cpu(p);
+	jtag_tclk_clr(p);
+
+	/* set RW to read */
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+	jtag_dr_shift_16(p, 0x2409);
+	jtag_ir_shift(p, IR_DATA_QUICK);
+
+	for (index = 0; index < length; index++) {
+		jtag_tclk_set(p);
+		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_set(p);
+	jlf16_release_cpu(p);
+	jtag_write_reg(p, 0, pc_bak);
+}
+
+/* Writes one byte/word at a given address
+ * format : 8-byte, 16-word
+ * address: address to be written
+ * data   : data to write
+ */
+static void jlf16_write_mem(struct jtdev *p, unsigned int format,
+		    address_t address, uint16_t data)
+{	// SLAU320AJ name: WriteMem
+	dbg_printc("%dbit %04x <- %04x\n", format, address, data);
+
+	jlf16_halt_cpu(p);
+	jtag_tclk_clr(p);
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+
+	if (format == 16)
+		/* Set word write */
+		jtag_dr_shift_16(p, 0x2408);
+	else
+		/* Set byte write */
+		jtag_dr_shift_16(p, 0x2418);
+
+	jtag_ir_shift(p, IR_ADDR_16BIT);
+
+	/* Set addr */
+	jtag_dr_shift_16(p, address);
+	jtag_ir_shift(p, IR_DATA_TO_ADDR);
+
+	/* Shift in 16 bits */
+	jtag_dr_shift_16(p, data);
+	jtag_tclk_set(p);
+	jlf16_release_cpu(p);
+}
+
+/* Writes an array of words into target memory
+ * address: address to write to
+ * length : number of word to write
+ * data   : data to write
+ */
+static void jlf16_write_mem_quick(struct jtdev *p, address_t address,
+			  unsigned int length, const uint16_t *data)
+{	// SLAU320AJ name: WriteMemQuick
+	unsigned int index;
+
+	dbg_printc("%04x..%04x\n", address, address+length*2);
+
+	/* Initialize writing */
+	jtag_write_reg(p, 0, address-4);
+	jlf16_halt_cpu(p);
+	jtag_tclk_clr(p);
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+
+	/* 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]);
+
+		/* Increment PC by 2 */
+		jtag_tclk_set(p);
+		jtag_tclk_clr(p);
+	}
+
+	jtag_tclk_set(p);
+	jlf16_release_cpu(p);
+}
+
+/* Execute a Power-Up Clear (PUC) using JTAG CNTRL SIG register
+ * return: JTAG ID
+ */
+static unsigned int jlf16_execute_puc(struct jtdev *p)
+{	// SLAU320AJ name: ExecutePOR
+	unsigned int jtag_id;
+
+	dbg_printc("\n");
+
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+
+	/* Apply and remove reset */
+	jtag_dr_shift_16(p, 0x2C01);
+	jtag_dr_shift_16(p, 0x2401);
+	jtag_tclk_clr(p);
+	jtag_tclk_set(p);
+	jtag_tclk_clr(p);
+	jtag_tclk_set(p);
+	jtag_tclk_clr(p);
+	jtag_tclk_set(p); // TODO: ???
+
+	/* Read jtag id */
+	jtag_id = jtag_ir_shift(p, IR_ADDR_CAPTURE);
+
+	//jtag_tclk_set(p); // TODO: ???
+
+	/* Disable watchdog on target device */
+	//jtag_write_mem(p, 16, 0x0120, 0x5A80); // FIXME
+
+	return jtag_id;
+}
+
+/* Release the target device from JTAG control
+ * address: 0xFFFE - perform Reset,
+ *                   load Reset Vector into PC
+ *          0xFFFF - start execution at current
+ *                   PC position
+ *          other  - load Address into PC
+ */
+static void jlf16_release_device(struct jtdev *p, address_t address)
+{	// SLAU320AJ name: ReleaseDevice
+	switch (address) {
+		case 0xffff: /* Nothing to do */
+			dbg_printc("BOR\n");
+			break;
+		case 0xfffe: /* Perform reset */
+			dbg_printc("SRST\n");
+			/* delete all breakpoints */
+			jtag_set_breakpoint(p,-1,0);
+			/* issue reset */
+			jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+			jtag_dr_shift_16(p, 0x2C01);
+			jtag_dr_shift_16(p, 0x2401);
+			break;
+		default: /* Set target CPU's PC */
+			dbg_printc("PC: %04x\n", address);
+			jtag_write_reg(p, 0, address);
+			break;
+	}
+
+	jlf16_set_instruction_fetch(p);
+
+	jtag_ir_shift(p, IR_EMEX_DATA_EXCHANGE);
+	jtag_dr_shift_16(p, BREAKREACT + READ);
+	jtag_dr_shift_16(p, 0x0000);
+
+	jtag_ir_shift(p, IR_EMEX_WRITE_CONTROL);
+	jtag_dr_shift_16(p, 0x000f);
+
+	jtag_ir_shift(p, IR_CNTRL_SIG_RELEASE);
+}
+
+/* Programs/verifies an array of words into a FLASH by using the
+ * FLASH controller. The JTAG FLASH register isn't needed.
+ * start_address: start in FLASH
+ * length       : number of words
+ * data         : pointer to data
+ */
+static void jlf16_write_flash(struct jtdev *p, address_t start_address,
+		      unsigned int length, const uint16_t *data)
+{	// SLAU320AJ name: WriteFLASH
+	unsigned int index;
+	unsigned int address;
+
+	dbg_printc("%04x..%04x\n", address, address+length*2);
+
+	address = start_address;
+	jlf16_halt_cpu(p);
+	jtag_tclk_clr(p);
+
+	/* Set RW to write */
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+	jtag_dr_shift_16(p, 0x2408);
+
+	/* FCTL1 register */
+	jtag_ir_shift(p, IR_ADDR_16BIT);
+	jtag_dr_shift_16(p, 0x0128); // FIXME
+
+	/* Enable FLASH write */
+	jtag_ir_shift(p, IR_DATA_TO_ADDR);
+	jtag_dr_shift_16(p, 0xA540);
+	jtag_tclk_set(p);
+	jtag_tclk_clr(p);
+
+	/* FCTL2 register */
+	jtag_ir_shift(p, IR_ADDR_16BIT);
+	jtag_dr_shift_16(p, 0x012A); // FIXME
+
+	/* Select MCLK as source, DIV=1 */
+	jtag_ir_shift(p, IR_DATA_TO_ADDR);
+	jtag_dr_shift_16(p, 0xA540);
+	jtag_tclk_set(p);
+	jtag_tclk_clr(p);
+
+	/* FCTL3 register */
+	jtag_ir_shift(p, IR_ADDR_16BIT);
+	jtag_dr_shift_16(p, 0x012C); // FIXME
+
+	/* Clear FCTL3 register */
+	jtag_ir_shift(p, IR_DATA_TO_ADDR);
+	jtag_dr_shift_16(p, 0xA500);
+	jtag_tclk_set(p);
+	jtag_tclk_clr(p);
+
+	for (index = 0; index < length; index++) {
+		/* Set RW to write */
+		jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+		jtag_dr_shift_16(p, 0x2408);
+
+		/* Set address */
+		jtag_ir_shift(p, IR_ADDR_16BIT);
+		jtag_dr_shift_16(p, address);
+
+		/* Set data */
+		jtag_ir_shift(p, IR_DATA_TO_ADDR);
+		jtag_dr_shift_16(p, data[index]);
+		jtag_tclk_set(p);
+		jtag_tclk_clr(p);
+
+		/* Set RW to read */
+		jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+		jtag_dr_shift_16(p, 0x2409);
+
+		/* provide TCLKs
+		 * min. 33 for F149 and F449
+		 */
+		p->f->jtdev_tclk_strobe(p, 35);
+		address += 2;
+
+		if (p->failed)
+			break;
+	}
+
+	/* Set RW to write */
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+	jtag_dr_shift_16(p, 0x2408);
+
+	/* FCTL1 register */
+	jtag_ir_shift(p, IR_ADDR_16BIT);
+	jtag_dr_shift_16(p, 0x0128); // FIXME
+
+	/* Disable FLASH write */
+	jtag_ir_shift(p, IR_DATA_TO_ADDR);
+	jtag_dr_shift_16(p, 0xA500);
+	jtag_tclk_set(p);
+	jtag_tclk_clr(p);
+
+	/* Reset FCTL3 */
+	jtag_ir_shift(p, IR_ADDR_16BIT);
+	jtag_dr_shift_16(p, 0x012C); // FIXME
+	jtag_ir_shift(p, IR_DATA_TO_ADDR);
+	jtag_dr_shift_16(p, 0xA510);
+	jtag_tclk_set(p);
+}
+
+/* Performs a mass erase (with and w/o info memory) or a segment erase of a
+ * FLASH module specified by the given mode and address. Large memory devices
+ * get additional mass erase operations to meet the spec.
+ * erase_mode   : ERASE_MASS, ERASE_MAIN, ERASE_SGMT
+ * erase_address: address within the selected segment
+ */
+static void jlf16_erase_flash(struct jtdev *p, unsigned int erase_mode,
+		      address_t erase_address)
+{	// SLAU320AJ name: EraseFLASH
+	unsigned int number_of_strobes = 4820;	/* default for segment erase */
+	unsigned int loop_counter;
+	unsigned int max_loop_count = 1;	/* erase cycle repeating for mass erase */
+
+	dbg_printc("%04x: %04x\n", erase_mode, erase_address);
+
+	if ((erase_mode == JTAG_ERASE_MASS) || (erase_mode == JTAG_ERASE_MAIN)) {
+		number_of_strobes = 5300;	/* Larger Flash memories require */
+		max_loop_count = 19;		/* additional cycles for erase. */
+		erase_address = 0xfffe;		/* overwrite given address */
+	}
+
+	for (loop_counter = max_loop_count; loop_counter > 0; loop_counter--) {
+		jlf16_halt_cpu(p);
+		jtag_tclk_clr(p);
+
+		/* Set RW to write */
+		jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+		jtag_dr_shift_16(p, 0x2408);
+
+		/* FCTL1 address */
+		jtag_ir_shift(p, IR_ADDR_16BIT);
+		jtag_dr_shift_16(p, 0x0128); // FIXME
+
+		/* Enable erase mode */
+		jtag_ir_shift(p, IR_DATA_TO_ADDR);
+		jtag_dr_shift_16(p, erase_mode); // FIXME?
+		jtag_tclk_set(p);
+		jtag_tclk_clr(p);
+
+		/* FCTL2 address */
+		jtag_ir_shift(p, IR_ADDR_16BIT);
+		jtag_dr_shift_16(p, 0x012A); // FIXME
+
+		/* MCLK is source, DIV=1 */
+		jtag_ir_shift(p, IR_DATA_TO_ADDR);
+		jtag_dr_shift_16(p, 0xA540);
+		jtag_tclk_set(p);
+		jtag_tclk_clr(p);
+
+		/* FCTL3 address */
+		jtag_ir_shift(p, IR_ADDR_16BIT);
+		jtag_dr_shift_16(p, 0x012C); // FIXME
+
+		/* Clear FCTL3 */
+		jtag_ir_shift(p, IR_DATA_TO_ADDR);
+		jtag_dr_shift_16(p, 0xA500);
+		jtag_tclk_set(p);
+		jtag_tclk_clr(p);
+
+		/* Set erase address */
+		jtag_ir_shift(p, IR_ADDR_16BIT);
+		jtag_dr_shift_16(p, erase_address);
+
+		/* Dummy write to start erase */
+		jtag_ir_shift(p, IR_DATA_TO_ADDR);
+		jtag_dr_shift_16(p, 0x55AA);
+		jtag_tclk_set(p);
+		jtag_tclk_clr(p);
+
+		/* Set RW to read */
+		jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+		jtag_dr_shift_16(p, 0x2409);
+
+		/* provide TCLKs */
+		p->f->jtdev_tclk_strobe(p, number_of_strobes);
+
+		/* Set RW to write */
+		jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+		jtag_dr_shift_16(p, 0x2408);
+
+		/* FCTL1 address */
+		jtag_ir_shift(p, IR_ADDR_16BIT);
+		jtag_dr_shift_16(p, 0x0128); // FIXME
+
+		/* Disable erase */
+		jtag_ir_shift(p, IR_DATA_TO_ADDR);
+		jtag_dr_shift_16(p, 0xA500);
+		jtag_tclk_set(p);
+		jtag_tclk_clr(p);
+
+		/* Reset FCTL3 */
+		jtag_ir_shift(p, IR_ADDR_16BIT);
+		jtag_dr_shift_16(p, 0x012C); // FIXME
+		jtag_ir_shift(p, IR_DATA_TO_ADDR);
+		jtag_dr_shift_16(p, 0xA510);
+		jtag_tclk_set(p);
+
+		jlf16_release_cpu(p);
+	}
+}
+
+/* Reads a register from the target CPU */
+static address_t jlf16_read_reg(struct jtdev *p, int reg)
+{	// libmsp430 BIOS name: ReadCpuReg
+	unsigned int value;
+
+	dbg_printc("%d\n", reg);
+
+	/* Set CPU into instruction fetch mode */
+	jlf16_set_instruction_fetch(p);
+
+	/* CPU controls RW & BYTE */
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+	jtag_dr_shift_16(p, 0x3401);
+
+	jtag_ir_shift(p, IR_DATA_16BIT);
+
+	/* "jmp $-4" instruction */
+	/* PC - 4 -> PC          */
+	/* needs 2 clock cycles  */
+	jtag_dr_shift_16(p, 0x3ffd);
+	jtag_tclk_clr(p);
+	jtag_tclk_set(p);
+	jtag_tclk_clr(p);
+	jtag_tclk_set(p);
+
+	/* "mov Rn,&0x01fe" instruction
+	 * Rn -> &0x01fe
+	 * PC is advanced 4 bytes by this instruction
+	 * needs 4 clock cycles
+	 * it's a ROM address, write has no effect, but
+	 * the registers value is placed on the databus
+	 */
+	jtag_dr_shift_16(p, 0x4082 | (((unsigned int)reg << 8) & 0x0f00) );
+	jtag_tclk_clr(p);
+	//jtag_ir_shift(p, IR_DATA_CAPTURE); // TODO: ???
+	jtag_tclk_set(p);
+	//jtag_ir_shift(p, IR_DATA_16BIT); // TODO: ???
+	jtag_dr_shift_16(p, 0x01fe);
+	jtag_tclk_clr(p);
+	jtag_tclk_set(p);
+	jtag_tclk_clr(p);
+	jtag_tclk_set(p);
+	/* older code did an extra clock cycle -- don't do this! will put the
+	 * current instruction word on the data bus instead of the register value
+	 * on the G2452, making it useless. the clock cycles are still required to
+	 * move to the next instruction, but those should be done later. */
+	/*jtag_tclk_clr(p);
+	jtag_tclk_set(p);*/
+
+	/* Read databus which contains the registers value */
+	jtag_ir_shift(p, IR_DATA_CAPTURE);
+	value = jtag_dr_shift_16(p, 0x0000);
+
+	jtag_tclk_clr(p);
+
+	/* JTAG controls RW & BYTE */
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+	jtag_dr_shift_16(p, 0x2401);
+
+	jtag_tclk_set(p);
+
+	dbg_printc("%d -> %04x\n", reg, value);
+
+	/* Return value read from register */
+	return value;
+}
+
+/* Writes a value into a register of the target CPU */
+static void jlf16_write_reg(struct jtdev *p, int reg, address_t value)
+{	// SLAU320AJ name: SetPC
+	/* Set CPU into instruction fetch mode */
+	dbg_printc("%d <- %04x\n", reg, value);
+
+	jlf16_set_instruction_fetch(p);
+
+	/* CPU controls RW & BYTE */
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+	jtag_dr_shift_16(p, 0x3401);
+
+	jtag_ir_shift(p, IR_DATA_16BIT);
+
+	/* "jmp $-4" instruction */
+	/* PC - 4 -> PC          */
+	/* needs 4 clock cycles  */
+	jtag_dr_shift_16(p, 0x3ffd);
+	jtag_tclk_clr(p);
+	jtag_tclk_set(p);
+	jtag_tclk_clr(p);
+	jtag_tclk_set(p);
+
+	/* "mov #value,Rn" instruction
+	 * value -> Rn
+	 * PC is advanced 4 bytes by this instruction
+	 * needs 2 clock cycles
+	 */
+	jtag_dr_shift_16(p, 0x4030 | (reg & 0x000f) );
+	jtag_tclk_clr(p);
+	jtag_tclk_set(p);
+	jtag_dr_shift_16(p, value);
+	jtag_tclk_clr(p);
+	jtag_tclk_set(p);
+
+	// TODO: ???
+	//jtag_ir_shift(p, IR_ADDR_CAPTURE);
+	//jtag_tclk_clr(p);
+
+	/* JTAG controls RW & BYTE */
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+	jtag_dr_shift_16(p, 0x2401);
+}
+
+/*----------------------------------------------------------------------------*/
+static void jlf16_single_step( struct jtdev *p )
+{	// libmsp430 BIOS name: SingleStep
+	unsigned int loop_counter;
+
+	dbg_printc("\n");
+
+	jlf16_set_instruction_fetch(p);
+
+	/* CPU controls RW & BYTE */
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+	jtag_dr_shift_16(p, 0x3401);
+
+	/* clock CPU until next instruction fetch cycle  */
+	/* failure after 10 clock cycles                 */
+	/* this is more than for the longest instruction */
+	jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
+	for (loop_counter = 10; loop_counter > 0; loop_counter--) {
+		jtag_tclk_clr(p);
+		jtag_tclk_set(p);
+		if ((jtag_dr_shift_16(p, 0x0000) & 0x0080) == 0x0080) {
+			break;
+		}
+	}
+
+	/* JTAG controls RW & BYTE */
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+	jtag_dr_shift_16(p, 0x2401);
+
+	jlf16_set_instruction_fetch(p);
+
+	if (loop_counter == 0) {
+		/* timeout reached */
+		printc_err("jtaglib_cpu16: single step failed\n");
+		p->failed = 1;
+	}
+}
+
+/*----------------------------------------------------------------------------*/
+static unsigned int jlf16_set_breakpoint( struct jtdev *p,int bp_num, address_t bp_addr )
+{
+	/* The breakpoint logic is explained in 'SLAU414c EEM.pdf' */
+	/* A good overview is given with Figure 1-1                */
+	/* MBx           is TBx         in EEM_defs.h              */
+	/* CPU Stop      is BREAKREACT  in EEM_defs.h              */
+	/* State Storage is STOR_REACT  in EEM_defs.h              */
+	/* Cycle Counter is EVENT_REACT in EEM_defs.h              */
+
+	unsigned int breakreact;
+
+	if (bp_num >= 8) {
+		/* there are no more than 8 breakpoints in EEM */
+		printc_err("jlf16_set_breakpoint: failed setting "
+			   "breakpoint %d at %04x\n", bp_num, bp_addr);
+		p->failed = 1;
+		return 0;
+	}
+
+	dbg_printc("num=%d addr=%04x\n", bp_num, bp_addr);
+
+	if (bp_num < 0) {
+		/* disable all breakpoints by deleting the BREAKREACT
+		 * register */
+		jtag_ir_shift(p, IR_EMEX_DATA_EXCHANGE);
+		jtag_dr_shift_16(p, BREAKREACT + WRITE);
+		jtag_dr_shift_16(p, 0x0000);
+		return 1;
+	}
+
+	/* set breakpoint */
+	jtag_ir_shift(p, IR_EMEX_DATA_EXCHANGE);
+	jtag_dr_shift_16(p, GENCTRL + WRITE);
+	jtag_dr_shift_16(p, EEM_EN + CLEAR_STOP + EMU_CLK_EN + EMU_FEAT_EN);
+
+	jtag_ir_shift(p, IR_EMEX_DATA_EXCHANGE); //repeating may not needed
+	jtag_dr_shift_16(p, 8*bp_num + MBTRIGxVAL + WRITE);
+	jtag_dr_shift_16(p, bp_addr);
+
+	jtag_ir_shift(p, IR_EMEX_DATA_EXCHANGE); //repeating may not needed
+	jtag_dr_shift_16(p, 8*bp_num + MBTRIGxCTL + WRITE);
+	jtag_dr_shift_16(p, MAB + TRIG_0 + CMP_EQUAL);
+
+	jtag_ir_shift(p, IR_EMEX_DATA_EXCHANGE); //repeating may not needed
+	jtag_dr_shift_16(p, 8*bp_num + MBTRIGxMSK + WRITE);
+	jtag_dr_shift_16(p, NO_MASK);
+
+	jtag_ir_shift(p, IR_EMEX_DATA_EXCHANGE); //repeating may not needed
+	jtag_dr_shift_16(p, 8*bp_num + MBTRIGxCMB + WRITE);
+	jtag_dr_shift_16(p, 1<<bp_num);
+
+	/* read the actual setting of the BREAKREACT register         */
+	/* while reading a 1 is automatically shifted into LSB        */
+	/* this will be undone and the bit for the new breakpoint set */
+	/* then the updated value is stored back                      */
+	jtag_ir_shift(p, IR_EMEX_DATA_EXCHANGE); //repeating may not needed
+	breakreact  = jtag_dr_shift_16(p, BREAKREACT + READ);
+	breakreact += jtag_dr_shift_16(p, 0x000);
+	breakreact  = (breakreact >> 1) | (1 << bp_num);
+	jtag_dr_shift_16(p, BREAKREACT + WRITE);
+	jtag_dr_shift_16(p, breakreact);
+	return 1;
+}
+
+/*----------------------------------------------------------------------------*/
+static unsigned int jlf16_cpu_state( struct jtdev *p )
+{	// libmsp430 BIOS name: WaitForEem(?)
+	jtag_ir_shift(p, IR_EMEX_READ_CONTROL);
+
+	if ((jtag_dr_shift_16(p, 0x0000) & 0x0080) == 0x0080) {
+		dbg_printc("halted\n");
+		return 1; /* halted */
+	} else {
+		dbg_printc("running\n");
+		return 0; /* running */
+	}
+}
+
+/*----------------------------------------------------------------------------*/
+static int jlf16_get_config_fuses( struct jtdev *p )
+{	// always the same?
+	jtag_ir_shift(p, IR_CONFIG_FUSES);
+
+	return jtag_dr_shift_8(p, 0);
+}
+
+/* ------------------------------------------------------------------------- */
+
+const struct jtaglib_funcs jlf_cpu16 = {
+	.jlf_get_device = jlf16_get_device,
+
+	.jlf_read_mem = jlf16_read_mem,
+	.jlf_read_mem_quick = jlf16_read_mem_quick,
+	.jlf_write_mem = jlf16_write_mem,
+	.jlf_write_mem_quick = jlf16_write_mem_quick,
+
+	.jlf_execute_puc = jlf16_execute_puc,
+	.jlf_release_device = jlf16_release_device,
+
+	.jlf_verify_mem = jlf16_verify_mem,
+	.jlf_erase_check = jlf16_erase_check,
+
+	.jlf_write_flash = jlf16_write_flash,
+	.jlf_erase_flash = jlf16_erase_flash,
+
+	.jlf_context_save = jtag_dev_default_context_save,
+	.jlf_context_restore = jtag_dev_default_context_restore,
+	.jlf_regs_update = jtag_dev_default_regs_update,
+	.jlf_regs_flush = jtag_dev_default_regs_flush,
+
+	.jlf_read_reg = jlf16_read_reg,
+	.jlf_write_reg = jlf16_write_reg,
+	.jlf_single_step = jlf16_single_step,
+	.jlf_set_breakpoint = jlf16_set_breakpoint,
+	.jlf_cpu_state = jlf16_cpu_state,
+	.jlf_get_config_fuses = jlf16_get_config_fuses,
+};
+

drivers/jtaglib_cpux.c

@@ -0,0 +1,7 @@
+
+#include "jtaglib.h"
+#include "jtaglib_defs.h"
+#include "output.h"
+
+const struct jtaglib_funcs jlf_cpux;
+

drivers/jtaglib_cpuxv2.c

@@ -0,0 +1,889 @@
+
+#include <stdbool.h>
+
+#include "jtaglib.h"
+#include "jtaglib_defs.h"
+#include "output.h"
+
+#define SAFE_FRAM_PC 0x0004
+
+#ifdef DEBUG_JTAGLIB
+#define dbg_printc(fmt, ...) printc_dbg("jlfxv2: %s:%d " fmt, __func__, __LINE__, ##__VA_ARGS__)
+#else
+#define dbg_printc(fmt, ...) do{}while(0)
+#endif
+
+static address_t jlfxv2_read_reg(struct jtdev *p, int reg);
+static void jlfxv2_write_reg(struct jtdev *p, int reg, address_t value);
+
+// FIXME:
+// * context save/restore:
+//   * pc gets written correctly on single-step
+//   * pc does NOT get written on 'run'
+//   * pc does NOT get read properly on run break
+// * implement flash write, erase, verify stuff
+// * check how ^ works in practice on flash & FRAM devices
+// * single step: not working!
+// * breakpoints are a big TODO
+
+static int jlfxv2_check_full_emu_state_ex(struct jtdev *p, const char* fnname)
+{	// see SLAU320AJ ExecutePOR
+	uint16_t dr;
+	uint8_t jtag_id;
+
+	for (int i = 0; i < 1/*10*/; ++i) {
+		jtag_id = jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
+		if (((dr = jtag_dr_shift_16(p, 0)) & 0x0301) == 0x0301) {
+			return 1; // OK
+		}
+
+		printc_err("jlfxv2: %s: not in full emu state, while expected!"
+				" (dr=%04x jid=%02x)\n", fnname, dr, jtag_id);
+	}
+	p->failed = 1;
+
+	return 0;
+}
+#define jlfxv2_check_full_emu_state(p) jlfxv2_check_full_emu_state_ex((p), __func__)
+
+static int jlfxv2_wait_sync_ex(struct jtdev *p, const char* fnname) {
+	uint16_t dr;
+	uint8_t jtag_id;
+
+	jtag_id = jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
+	for (int i = 0; i < 50; ++i) {
+		if ((dr = jtag_dr_shift_16(p, 0)) & 0x0200)
+			return 1;
+	}
+
+	printc_err("jlfxv2: %s: couldn't sync! (dr=%04x jid=%02x)\n", fnname, dr, jtag_id);
+	return 0;
+}
+#define jlfxv2_wait_sync(p) jlfxv2_wait_sync_ex((p), __func__)
+
+
+static uint8_t bitswap_nyb(uint8_t in) {
+	return ((in >> 3) & 1) | ((in >> 1) & 2) | ((in << 1) & 4) | ((in << 3) & 8);
+}
+static uint8_t bitswap(uint8_t in) {
+	return (bitswap_nyb(in&0xf) << 4) | bitswap_nyb(in>>4);
+}
+static address_t demangle_mab(address_t mab) {
+	address_t page = mab & 0xf0000;
+	address_t bot  = mab & 0x0ff00;
+	address_t top  = mab & 0x000ff;
+
+	address_t ret = page | (bot >> 8) | ((address_t)bitswap(top) << 8);
+	//dbg_printc("demangle: %05x -> %05x\n", mab, ret);
+	return ret;
+}
+
+static int jlfxv2_set_pc(struct jtdev *p, address_t addr)
+{
+	address_t addr_rb;
+
+	dbg_printc("set pc %05x\n", addr);
+
+	if (!jlfxv2_wait_sync(p)) return -1;
+	/*jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
+	jtag_dr_shift_16(p, 0);*/
+	jtag_tclk_clr(p);
+
+	jtag_ir_shift(p, IR_DATA_16BIT);
+	jtag_tclk_set(p);
+	jtag_dr_shift_16(p, 0x0080 | ((addr >> 8) & 0x0f00));
+	jtag_tclk_clr(p);
+
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+	jtag_dr_shift_16(p, 0x1400);
+
+	jtag_ir_shift(p, IR_DATA_16BIT);
+	jtag_tclk_clr(p);
+	jtag_tclk_set(p);
+	jtag_dr_shift_16(p, addr & 0xffff);
+	jtag_tclk_clr(p);
+	jtag_tclk_set(p);
+
+	jtag_dr_shift_16(p, 0x4303);
+	jtag_tclk_clr(p);
+	jtag_ir_shift(p, IR_ADDR_CAPTURE);
+	addr_rb = demangle_mab(jtag_dr_shift_20(p, 0));
+	dbg_printc("set pc MAB -> %05x%s\n", addr_rb,
+			(addr!=addr_rb)?", aieee!":"");
+
+	return (addr == addr_rb) ? 0 : -1;
+}
+/*static address_t jlfxv2_get_pc(struct jtdev *p)
+{
+	jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
+	jtag_dr_shift_16(p, 0);
+	jtag_ir_shift(p, IR_ADDR_CAPTURE);
+	jtag_dr_shift_20(p, 0);
+
+	address_t addr = jtag_dr_shift_20(p, 0);
+
+	dbg_printc("get pc %05x\n", addr);
+
+	return addr;
+}*/
+
+/* Compares the computed PSA (Pseudo Signature Analysis) value to the PSA
+ * value shifted out from the target device. It is used for very fast data
+ * block write or erasure verification.
+ * start_address: start of data
+ * length       : number of data
+ * data         : pointer to data, 0 for erase check
+ * RETURN       : 1 - comparison was successful
+ *                0 - otherwise
+ */
+static int jlfxv2_verify_mem(struct jtdev *p,
+			   unsigned int start_address,
+			   unsigned int length,
+			   const uint16_t *data)
+{	// SLAU320AJ name: VerifyMem/VerifyPSA
+	uint16_t crc, psaval;
+
+	jtag_execute_puc(p);
+
+	crc = start_address - 2;
+	if (jlfxv2_set_pc(p, start_address) < 0) return -1;
+
+	jtag_tclk_set(p);
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+	jtag_dr_shift_16(p, 0x0501);
+	jtag_ir_shift(p, IR_DATA_16BIT);
+	jtag_dr_shift_16(p, start_address - 2);
+	jtag_ir_shift(p, IR_DATA_PSA);
+
+	for (unsigned int addr = 0; addr < length; ++addr) {
+		if (crc & 0x8000) {
+			crc ^= 0x0805;
+			crc <<= 1;
+			crc |= 1;
+		} else {
+			crc <<= 1;
+		}
+
+		if (data) crc ^= data[addr];
+		else crc ^= 0xffff;
+
+		jtag_tclk_clr(p);
+		/* Go through DR path without shifting data in/out */
+		jtag_tms_sequence(p, 6, 0x19); /* TMS=1 0 0 1 1 0 ; 6 clocks */
+		jtag_tclk_set(p);
+	}
+
+	jtag_ir_shift(p, IR_SHIFT_OUT_PSA);
+	psaval = jtag_dr_shift_16(p, 0);
+
+	jtag_execute_puc(p);
+
+	return (psaval == crc) ? 1 : 0;
+}
+
+/* ------------------------------------------------------------------------- */
+
+static int jlfxv2_erase_check(struct jtdev *p, unsigned int start_address,
+			   unsigned int length)
+{
+	return jlfxv2_verify_mem(p, start_address, length, NULL);
+}
+
+static unsigned int jlfxv2_get_device(struct jtdev *p)
+{	// SLAU320AJ name: GetDevice (and SyncJtag_AssertPor)
+	unsigned int jtag_id = 0;
+	int i;
+
+	dbg_printc("jlfxv2: get_device\n");
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+	jtag_dr_shift_16(p, 0x1501);
+
+	jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
+	for (i = 0; i < 50; ++i) {
+		if ((jtag_dr_shift_16(p, 0) & (1<<9)) == (1<<9))
+			break;
+	}
+
+	if (i == 50) {
+		printc_err("jlfxv2_get_device: failed\n");
+		p->failed = 1;
+		return 0;
+	}
+
+	jtag_id = jtag_execute_puc(p);
+	return jtag_id;
+}
+
+static void jlfxv2_read_mem_quick(struct jtdev *p, address_t address,
+			 unsigned int length, uint16_t *data);
+
+/* Reads one byte/word from a given address
+ * format : 8-byte, 16-word
+ * address: address of memory
+ * return : content of memory
+ */
+static uint16_t jlfxv2_read_mem(struct jtdev *p, unsigned int format, address_t address)
+{	// SLAU320AJ name: ReadMem
+	uint16_t r = 0;
+
+	dbg_printc("read mem %05x\n", address);
+	if (!jlfxv2_check_full_emu_state(p))
+		return 0;
+
+	// the code below (an attempt at implementing the real read_mem) doesn't
+	// work, so let's use read_mem_quick as a backup
+	jlfxv2_read_mem_quick(p, address ^ (address & 1), 1, &r);
+	if (format == 8) {
+		if (address & 1) return (r >> 8) & 0xff;
+		else return r & 0xff;
+	} else return r;
+
+	/*//jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
+	//uint16_t dr = jtag_dr_shift_16(p, 0);
+	jtag_tclk_clr(p);
+	//jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+	//jtag_dr_shift_16(p, (format == 16) ? 0x0501 : 0x0511);
+	jtag_ir_shift(p, IR_ADDR_16BIT);
+	jtag_dr_shift_20(p, address ^ (address & 1));
+	//jtag_ir_shift(p, IR_DATA_TO_ADDR);
+	jtag_tclk_set(p);
+	jtag_tclk_clr(p);
+	jtag_ir_shift(p, IR_DATA_CAPTURE);
+
+	r = jtag_dr_shift_16(p, 0);
+
+	jtag_tclk_set(p);
+	jtag_tclk_clr(p);
+	jtag_tclk_set(p);
+
+	if (format == 8) {
+		if (address & 1) return (r >> 8) & 0xff;
+		else return r & 0xff;
+	} else return r;*/
+}
+
+/* Reads an array of words from target memory
+ * address: address to read from
+ * length : number of word to read
+ * data   : memory to write to
+ */
+static void jlfxv2_read_mem_quick(struct jtdev *p, address_t address,
+			 unsigned int length, uint16_t *data)
+{	// SLAU320AJ name: ReadMemQuick
+	if (!jlfxv2_check_full_emu_state(p))
+		return;
+
+	dbg_printc("read mem quick %05x..%05x\n", address, address+length*2);
+	if (jlfxv2_set_pc(p, address) < 0) return;
+
+	//jtag_tclk_set(p);
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+	jtag_dr_shift_16(p, 0x0501);
+	jtag_tclk_set(p);
+	jtag_ir_shift(p, IR_ADDR_CAPTURE);
+	jtag_ir_shift(p, IR_DATA_QUICK);
+
+	for (unsigned int i = 0; i < length; ++i) {
+		jtag_tclk_set(p);
+		jtag_tclk_clr(p);
+		data[i] = jtag_dr_shift_16(p, 0);
+	}
+
+	jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
+	jtag_dr_shift_16(p, 0);
+
+	jlfxv2_set_pc(p, SAFE_FRAM_PC);
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+	jtag_dr_shift_16(p, 0x0501);
+	jtag_tclk_set(p);
+	jtag_ir_shift(p, IR_ADDR_CAPTURE);
+}
+
+static void jlfxv2_write_mem_quick(struct jtdev *p, address_t address,
+			  unsigned int length, const uint16_t *data);
+
+/* Writes one byte/word at a given address
+ * format : 8-byte, 16-word
+ * address: address to be written
+ * data   : data to write
+ */
+static void jlfxv2_write_mem(struct jtdev *p, unsigned int format,
+		    address_t address, uint16_t data)
+{	// SLAU320AJ name: WriteMem
+	dbg_printc("write mem: %d %06x <- %04x\n", format, address, data);
+
+	if (format == 8) {
+		p->failed = 1;
+		printc_err("jlfxv2 write mem: byte access not yet implemented!\n");
+		return;
+	}
+
+	// same story as with read_mem
+	jlfxv2_write_mem_quick(p, address, 1, &data);
+
+	if (!jlfxv2_check_full_emu_state(p))
+		return;
+
+	/*jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
+	uint16_t dr = jtag_dr_shift_16(p, 0);
+
+	dbg_printc("write mem %d: %06x<-%04x: dr=%04x\n", format, address, data, dr);
+
+	jtag_tclk_clr(p);
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+	jtag_dr_shift_16(p, (format == 16) ? 0x0500 : 0x0510);
+	jtag_ir_shift(p, IR_ADDR_16BIT);
+	jtag_dr_shift_20(p, address);
+	jtag_tclk_set(p);
+	jtag_ir_shift(p, IR_DATA_TO_ADDR);
+	jtag_dr_shift_16(p, data);
+	jtag_tclk_clr(p);
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+	jtag_dr_shift_16(p, 0x0501);
+
+	jtag_tclk_set(p);
+	jtag_tclk_clr(p);
+	jtag_tclk_set(p);*/
+}
+
+/* Writes an array of words into target memory
+ * address: address to write to
+ * length : number of word to write
+ * data   : data to write
+ */
+static void jlfxv2_write_mem_quick(struct jtdev *p, address_t address,
+			  unsigned int length, const uint16_t *data)
+{	// SLAU320AJ name: WriteMemQuick
+	/*for (unsigned int i = 0; i < length; ++i) {
+		jlfxv2_write_mem(p, 16, address + i*2, data[i]);
+	}*/
+
+	dbg_printc("write mem quick: %05x..%05x\n", address, address+length*2);
+
+	if (!jlfxv2_check_full_emu_state(p))
+		return;
+
+	if (jlfxv2_set_pc(p, address) < 0) return;
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+	jtag_dr_shift_16(p, 0x0500);
+	jtag_tclk_set(p);
+
+	jtag_ir_shift(p, IR_DATA_QUICK);
+
+
+	for (unsigned int i = 0; i < length; ++i) {
+		jtag_tclk_set(p);
+		jtag_dr_shift_16(p, data[i]);
+		jtag_tclk_clr(p);
+	}
+
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+	jtag_dr_shift_16(p, 0x0501);
+	jtag_tclk_set(p);
+
+	jlfxv2_set_pc(p, SAFE_FRAM_PC);
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+	jtag_dr_shift_16(p, 0x0501);
+	jtag_tclk_set(p);
+	jtag_ir_shift(p, IR_ADDR_CAPTURE);
+}
+
+/* Execute a Power-Up Clear (PUC) using JTAG CNTRL SIG register
+ * return: JTAG ID
+ */
+static unsigned int jlfxv2_execute_puc(struct jtdev *p)
+{	// SLAU320AJ name: ExecutePOR
+	unsigned int jtag_id;
+
+	dbg_printc("execute_puc\n");
+
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+	jtag_dr_shift_16(p, 0x1501);
+
+	if (!jlfxv2_wait_sync(p)) return -1;
+
+	jtag_id = jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
+
+	// empty CPU pipeline
+	jtag_tclk_clr(p);
+	jtag_tclk_set(p);
+
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+
+	/* Apply and remove reset */
+	jtag_dr_shift_16(p, 0x0C01);
+	delay_ms(40);
+	jtag_dr_shift_16(p, 0x0401);
+
+	if (jtag_id == 0x91 || jtag_id == 0X99 || jtag_id == 0x98) {
+		jtag_ir_shift(p, IR_DATA_16BIT);
+
+		jtag_tclk_clr(p);
+		jtag_tclk_set(p);
+		jtag_tclk_clr(p);
+		jtag_tclk_set(p);
+
+		jtag_dr_shift_16(p, SAFE_FRAM_PC);
+
+		jtag_tclk_clr(p);
+		jtag_tclk_set(p);
+
+		if (jtag_id == 0x91) {
+			jtag_tclk_clr(p);
+			jtag_tclk_set(p);
+		}
+
+		jtag_ir_shift(p, IR_DATA_CAPTURE);
+	} else { // TODO: this if jtag_id == 0x91, else other branch?
+		jtag_tclk_clr(p);
+		jtag_tclk_set(p);
+		jtag_tclk_clr(p);
+		jtag_tclk_set(p);
+		jtag_tclk_clr(p);
+		jtag_tclk_set(p);
+	}
+
+	// two more cycles to release CPU internal POR delay signals
+	jtag_tclk_clr(p);
+	jtag_tclk_set(p);
+	jtag_tclk_clr(p);
+	jtag_tclk_set(p);
+
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+	jtag_dr_shift_16(p, 0x0501);
+	jtag_tclk_clr(p);
+	jtag_tclk_set(p);
+
+	return jtag_id;
+}
+
+/* Release the target device from JTAG control
+ * address: 0xFFFE - perform Reset,
+ *                   load Reset Vector into PC
+ *          0xFFFF - start execution at current
+ *                   PC position
+ *          other  - load Address into PC
+ */
+static void jlfxv2_release_device(struct jtdev *p, address_t address)
+{	// SLAU320AJ name: ReleaseDevice. from Replicator430 code, not in the PDF
+	switch (address) {
+	case 0xffff: // BOR
+		dbg_printc("jlfxv2: release device: BOR\n");
+
+		jtag_ir_shift(p, IR_TEST_REG);
+		jtag_dr_shift_16(p, 0x0200);
+		delay_ms(5);
+		break;
+	case 0xfffe: // reset
+		dbg_printc("jlfxv2: release device: SRST\n");
+
+		jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+		jtag_dr_shift_16(p, 0x0C01);
+		delay_ms(40);
+		jtag_dr_shift_16(p, 0x0401);
+		jtag_ir_shift(p, IR_CNTRL_SIG_RELEASE);
+		break;
+	default:
+		dbg_printc("jlfxv2: release device: PC=%05x\n", address);
+
+		jlfxv2_set_pc(p, address);
+		jtag_tclk_set(p);
+		jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+		jtag_dr_shift_16(p, 0x0401);
+		jtag_ir_shift(p, IR_ADDR_CAPTURE);
+
+		jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
+		jtag_tclk_clr(p);
+		if (jtag_dr_shift_16(p, 0) & 2/*CNTRL_SIG_HALT*/) {
+			jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+			jtag_dr_shift_16(p, 0x0403);
+		}
+		jtag_tclk_set(p);
+
+		jtag_ir_shift(p, IR_CNTRL_SIG_RELEASE);
+		break;
+	}
+}
+
+/* Programs/verifies an array of words into a FLASH by using the
+ * FLASH controller. The JTAG FLASH register isn't needed.
+ * start_address: start in FLASH
+ * length       : number of words
+ * data         : pointer to data
+ */
+static void jlfxv2_write_flash(struct jtdev *p, address_t start_address,
+		      unsigned int length, const uint16_t *data)
+{	// SLAU320AJ name: WriteFLASH
+	// TODO: implement!
+}
+
+/* Performs a mass erase (with and w/o info memory) or a segment erase of a
+ * FLASH module specified by the given mode and address. Large memory devices
+ * get additional mass erase operations to meet the spec.
+ * erase_mode   : ERASE_MASS, ERASE_MAIN, ERASE_SGMT
+ * erase_address: address within the selected segment
+ */
+static void jlfxv2_erase_flash(struct jtdev *p, unsigned int erase_mode,
+		      address_t erase_address)
+{	// SLAU320AJ name: EraseFLASH
+	// TODO: implement!
+}
+
+/* Reads a register from the target CPU */
+static address_t jlfxv2_read_reg(struct jtdev *p, int reg)
+{	// libmsp430 BIOS name: ReadCpuReg
+	uint16_t reglo, reghi;
+	uint16_t jtag_id, jmb_addr;
+	const bool alt_addr = false;//true;
+
+	if (reg == 3) return 0; // CG
+
+	dbg_printc("read reg %d\n", reg);
+	if (!jlfxv2_check_full_emu_state(p))
+		return 0;
+
+	jtag_id = jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
+	jtag_tclk_clr(p);
+	jtag_ir_shift(p, IR_DATA_16BIT);
+	jtag_tclk_set(p);
+	jtag_dr_shift_16(p, ((reg << 8) & 0x0f00) | 0x0060);
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+	jtag_dr_shift_16(p, 0x1401);
+	jtag_ir_shift(p, IR_DATA_16BIT);
+	jtag_tclk_clr(p);
+	jtag_tclk_set(p);
+
+	if (alt_addr) {
+		jtag_dr_shift_16(p, 0x0ff6);
+	} else {
+		jmb_addr = (jtag_id == 0x98) ? 0x14c : 0x18c;
+		jtag_dr_shift_16(p, jmb_addr);
+	}
+	jtag_tclk_clr(p);
+	jtag_tclk_set(p);
+	jtag_dr_shift_16(p, 0x3ffd);
+	jtag_tclk_clr(p);
+
+	if (alt_addr) {
+		jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+		jtag_dr_shift_16(p, 0x0501);
+	}
+
+	jtag_ir_shift(p, IR_DATA_CAPTURE);
+	jtag_tclk_set(p);
+	reglo = jtag_dr_shift_16(p, 0);
+	jtag_tclk_clr(p);
+	jtag_tclk_set(p);
+	reghi = jtag_dr_shift_16(p, 0);
+	jtag_tclk_clr(p);
+	jtag_tclk_set(p);
+	jtag_tclk_clr(p);
+	jtag_tclk_set(p);
+	jtag_tclk_clr(p);
+	jtag_tclk_set(p);
+
+	if (!alt_addr) {
+		jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+		jtag_dr_shift_16(p, 0x0501);
+	}
+
+	jtag_tclk_clr(p);
+	jtag_ir_shift(p, IR_DATA_CAPTURE);
+	jtag_tclk_set(p);
+
+	dbg_printc("read reg %d: lo=%04x hi=%04x\n", reg, reglo, reghi);
+
+	jlfxv2_set_pc(p, SAFE_FRAM_PC);
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+	jtag_dr_shift_16(p, 0x0501);
+	jtag_tclk_set(p);
+	jtag_ir_shift(p, IR_ADDR_CAPTURE);
+
+	return reglo | ((address_t)reghi << 16);
+}
+
+/* Writes a value into a register of the target CPU */
+static void jlfxv2_write_reg(struct jtdev *p, int reg, address_t value)
+{	// SLAU320AJ name: SetPC
+	if (reg == 0 || reg == 3) return; // pc, cg
+
+	dbg_printc("write reg %d %06x\n", reg, value);
+
+	if (!jlfxv2_check_full_emu_state(p)) return;
+
+	/*jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
+	jtag_dr_shift_16(p, 0);*/
+
+	jtag_tclk_clr(p);
+	jtag_ir_shift(p, IR_DATA_16BIT);
+	jtag_tclk_set(p);
+	jtag_dr_shift_16(p, 0x0080 | ((value >> 8) & 0x0f00) | (reg & 0xf));
+
+	//jtag_tclk_clr(p);
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+	jtag_dr_shift_16(p, 0x1401/*1400*/);
+	jtag_ir_shift(p, IR_DATA_16BIT);
+	jtag_tclk_clr(p);
+	jtag_tclk_set(p);
+	jtag_dr_shift_16(p, value & 0xffff);
+	jtag_tclk_clr(p);
+	jtag_tclk_set(p);
+
+	jtag_dr_shift_16(p, 0x3ffd); // rewind PC
+	jtag_tclk_clr(p);
+	jtag_tclk_set(p);
+	jtag_tclk_clr(p);
+
+	jtag_ir_shift(p, IR_ADDR_CAPTURE);
+	jtag_dr_shift_20(p, 0);
+	jtag_tclk_set(p);
+
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+	jtag_dr_shift_16(p, 0x0501);
+	jtag_tclk_clr(p);
+	jtag_tclk_set(p);
+	jtag_tclk_clr(p);
+
+	jtag_ir_shift(p, IR_DATA_CAPTURE);
+	jtag_tclk_set(p);
+	//jtag_dr_shift_20(p, 0);
+}
+
+/*----------------------------------------------------------------------------*/
+static void jlfxv2_single_step( struct jtdev *p )
+{	// libmsp430 BIOS name: SingleStep
+
+	int i, timeout;
+	uint16_t tmp;
+	dbg_printc("single step\n");
+
+	// TODO: fix this. it only performs an instruction fetch but not much else
+
+	/*jtag_ir_shift(p, IR_EMEX_READ_CONTROL);
+	timeout = 3000;
+	for (i = 0; i < timeout; ++i)
+		if (jtag_dr_shift_16(p, 0) & EEM_STOPPED)
+			break;
+	if (i == timeout) {
+		printc_err("jlfxv2_single_step: EEM timeout\n");
+		goto err;
+	}*/
+
+	jtag_ir_shift(p, IR_EMEX_WRITE_CONTROL);
+	jtag_dr_shift_16(p, EMU_CLK_EN | EEM_EN);
+
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+	jtag_dr_shift_16(p, 0x1501);
+	jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
+	timeout = 50;
+	for (i = 0; i < timeout; ++i) {
+		tmp = jtag_dr_shift_16(p, 0);
+		if (tmp != 0xffff && (tmp & 0x200) == 0x0200)
+			break;
+	}
+	if (i == timeout) {
+		printc_err("jlfxv2_single_step: JTAG sync timeout\n");
+		goto err;
+	}
+
+	jtag_ir_shift(p, IR_EMEX_WRITE_CONTROL);
+	jtag_dr_shift_16(p, EMU_CLK_EN | CLEAR_STOP);
+	jtag_dr_shift_16(p, EMU_CLK_EN | CLEAR_STOP | EEM_EN);
+
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+	jtag_dr_shift_16(p, 0x1501);
+
+	jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
+	timeout = 30000;
+	for (i = 0; i < timeout; ++i) {
+		if ((jtag_dr_shift_16(p, 0) & 8) == 0) break;
+
+		jtag_tclk_clr(p);
+		jtag_tclk_set(p);
+
+		jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
+	}
+	if (i == timeout) {
+		printc_err("jlfxv2_single_step: single-step timeout\n");
+		goto err;
+	}
+
+	jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
+	timeout = 10000;
+	for (i = 0; i < timeout; ++i) {
+		jtag_tclk_clr(p);
+		tmp = jtag_dr_shift_16(p, 0);
+		jtag_tclk_set(p);
+
+		if ((tmp & CNTRL_SIG_CPUSUSP) == CNTRL_SIG_CPUSUSP) break;
+	}
+	if (i == timeout) {
+		printc_err("jlfxv2_single_step: pipeline empty timeout\n");
+		goto err;
+	}
+
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+	jtag_dr_shift_16(p, 0x0501);
+
+	return;
+err:
+	p->failed = 1;
+}
+
+/*----------------------------------------------------------------------------*/
+static unsigned int jlfxv2_set_breakpoint( struct jtdev *p,int bp_num, address_t bp_addr )
+{
+	/* The breakpoint logic is explained in 'SLAU414c EEM.pdf' */
+	/* A good overview is given with Figure 1-1                */
+	/* MBx           is TBx         in EEM_defs.h              */
+	/* CPU Stop      is BREAKREACT  in EEM_defs.h              */
+	/* State Storage is STOR_REACT  in EEM_defs.h              */
+	/* Cycle Counter is EVENT_REACT in EEM_defs.h              */
+
+	// TODO: implement
+
+	return 0;
+}
+
+/*----------------------------------------------------------------------------*/
+static unsigned int jlfxv2_cpu_state( struct jtdev *p )
+{	// libmsp430 BIOS name: WaitForEem(?)
+	jtag_ir_shift(p, IR_EMEX_READ_CONTROL);
+
+	if ((jtag_dr_shift_16(p, 0x0000) & EEM_STOPPED) == EEM_STOPPED) {
+		dbg_printc("cpu_state: halted\n");
+		return 1; /* halted */
+	} else {
+		dbg_printc("cpu_state: running\n");
+		return 0; /* running */
+	}
+}
+
+/*----------------------------------------------------------------------------*/
+static int jlfxv2_get_config_fuses( struct jtdev *p )
+{	// always the same?
+	jtag_ir_shift(p, IR_CONFIG_FUSES);
+
+	return jtag_dr_shift_8(p, 0);
+}
+
+/* ------------------------------------------------------------------------- */
+
+static void jlfxv2_context_save(struct jtdev *p, bool after_puc) {
+	address_t wdtctl_a = (p->jtag_id == 0x89) ? 0x0120 : 0x015c,
+	          rst_vec = 0x0fffe,
+	          mab, entrypt_addr;
+
+	dbg_printc("context save, %s\n", after_puc?"after PUC":"normal");
+
+	jtag_ir_shift(p, IR_EMEX_WRITE_CONTROL);
+	jtag_dr_shift_16(p, EMU_FEAT_EN | EMU_CLK_EN | CLEAR_STOP);
+	jtag_ir_shift(p, IR_CNTRL_SIG_CAPTURE);
+	jtag_dr_shift_16(p, 0);
+
+	/* ReadMemWordXv2 */
+	jtag_tclk_clr(p);
+	jtag_ir_shift(p, IR_ADDR_16BIT);
+	jtag_dr_shift_20(p, wdtctl_a);
+	jtag_tclk_set(p);
+	jtag_tclk_clr(p);
+	jtag_ir_shift(p, IR_DATA_CAPTURE);
+	p->wdtctl = jtag_dr_shift_16(p, 0) & 0xff;
+	jtag_tclk_set(p);
+	jtag_tclk_clr(p);
+	jtag_tclk_set(p);
+	dbg_printc("WDTCTL: %04x\n", p->wdtctl);
+	/* ReadMemWordXv2 */
+	/* actually not using the result for this, but adding this makes it work
+	 * somehow */
+	jtag_tclk_clr(p);
+	jtag_ir_shift(p, IR_ADDR_16BIT);
+	jtag_dr_shift_20(p, rst_vec);
+	jtag_tclk_set(p);
+	jtag_tclk_clr(p);
+	jtag_ir_shift(p, IR_DATA_CAPTURE);
+	entrypt_addr = jtag_dr_shift_16(p, 0);
+	jtag_tclk_set(p);
+	jtag_tclk_clr(p);
+	jtag_tclk_set(p);
+	dbg_printc("ENTRYPOINT: %04x\n", entrypt_addr);
+
+	jtag_ir_shift(p, IR_ADDR_CAPTURE);
+	mab = demangle_mab(jtag_dr_shift_20(p, 0));
+	dbg_printc("PC MAB: %05x\n", mab);
+	//mab = rst_vec + 4;
+
+	/* back up program counter */
+	if (after_puc) {
+		p->regs[0] = rst_vec; /* yeah.. */
+	} else {
+		p->regs[0] = mab - 4;
+	}
+
+	if (p->regs[0] == rst_vec) {
+		p->regs[0] = jtag_read_mem(p, 16, rst_vec);
+		dbg_printc("ENTRY TRY1: %04x\n", p->regs[0]);
+		if (p->regs[0] == 0xffff || p->regs[0] == 0x3fff) {
+			p->regs[0] = jtag_read_mem(p, 16, rst_vec);
+			dbg_printc("ENTRY TRY2: %04x\n", p->regs[0]);
+		}
+	}
+
+	/* disable watchdog */
+	//p->wdtctl = jtag_read_mem(p, 16, wdtctl_a) & 0xff;
+	dbg_printc("WDTCTL: %04x\n", p->wdtctl);
+	jtag_write_mem(p, 16, wdtctl_a, p->wdtctl | 0x5a80);
+
+	/* also back up stack pointer & status register */
+	p->regs[1] = jtag_read_reg(p, 1);
+	p->regs[2] = jtag_read_reg(p, 2);
+
+	const static uint16_t data[] = {0x3fff,0x3fff,0x3fff};
+	jtag_write_mem_quick(p, 0x00004, sizeof(data)/sizeof(*data), data);
+}
+static void jlfxv2_context_restore(struct jtdev *p) {
+	dbg_printc("context restore\n");
+
+	/* basically the inverse of context_save */
+	address_t wdtctl_a = (p->jtag_id == 0x89) ? 0x0120 : 0x015c;
+
+	jtag_write_reg(p, 1, p->regs[1]);
+	jtag_write_reg(p, 2, p->regs[2]);
+
+	jtag_write_mem(p, 16, wdtctl_a, p->wdtctl | 0x5a00);
+
+	jlfxv2_set_pc(p, p->regs[0]);
+}
+
+/* ------------------------------------------------------------------------- */
+
+const struct jtaglib_funcs jlf_cpuxv2 = {
+	.jlf_get_device = jlfxv2_get_device,
+
+	.jlf_read_mem = jlfxv2_read_mem,
+	.jlf_read_mem_quick = jlfxv2_read_mem_quick,
+	.jlf_write_mem = jlfxv2_write_mem,
+	.jlf_write_mem_quick = jlfxv2_write_mem_quick,
+
+	.jlf_execute_puc = jlfxv2_execute_puc,
+	.jlf_release_device = jlfxv2_release_device,
+
+	.jlf_verify_mem = jlfxv2_verify_mem,
+	.jlf_erase_check = jlfxv2_erase_check,
+
+	.jlf_write_flash = jlfxv2_write_flash,
+	.jlf_erase_flash = jlfxv2_erase_flash,
+
+	.jlf_context_save = jlfxv2_context_save,
+	.jlf_context_restore = jlfxv2_context_restore,
+	.jlf_regs_update = jtag_dev_default_regs_update,
+	.jlf_regs_flush = jtag_dev_default_regs_flush,
+
+	.jlf_read_reg = jlfxv2_read_reg,
+	.jlf_write_reg = jlfxv2_write_reg,
+	.jlf_single_step = jlfxv2_single_step,
+	.jlf_set_breakpoint = jlfxv2_set_breakpoint,
+	.jlf_cpu_state = jlfxv2_cpu_state,
+	.jlf_get_config_fuses = jlfxv2_get_config_fuses,
+};
+

drivers/jtaglib_defs.h

@@ -0,0 +1,77 @@
+
+#include "jtaglib.h"
+#include "eem_defs.h"
+
+/* Flash erasing modes */
+#define JTAG_ERASE_MASS 0xA506
+#define JTAG_ERASE_MAIN 0xA504
+#define JTAG_ERASE_SGMT 0xA502
+
+#define JTAG_ID_IS_XV2(x) ((x)==0x91||(x)==0x95||(x)==0x98||(x)==0x99)
+
+/* Instructions for the JTAG control signal register in reverse bit order
+ */
+#define IR_CNTRL_SIG_16BIT	0xC8	/* 0x13 */
+#define IR_CNTRL_SIG_CAPTURE	0x28	/* 0x14 */
+#define IR_CNTRL_SIG_RELEASE	0xA8	/* 0x15 */
+#define IR_COREIP_ID		0xE8	/* 0x17 */
+/* Instructions for the JTAG data register */
+#define IR_DATA_16BIT		0x82	/* 0x41 */
+#define IR_DATA_CAPTURE		0x42	/* 0x42 */
+#define IR_DATA_QUICK		0xC2	/* 0x43 */
+/* Instructions for the JTAG address register */
+#define IR_ADDR_16BIT		0xC1	/* 0x83 */
+#define IR_ADDR_CAPTURE		0x21	/* 0x84 */
+#define IR_DATA_TO_ADDR		0xA1	/* 0x85 */
+#define IR_DEVICE_ID		0xE1	/* 0x87 */
+/* Instructions for the JTAG PSA mode */
+#define IR_DATA_PSA		0x22	/* 0x44 */
+#define IR_SHIFT_OUT_PSA	0x62	/* 0x46 */
+/* Instructions for the JTAG Fuse */
+#define IR_PREPARE_BLOW		0x44	/* 0x22 */
+#define IR_EX_BLOW		0x24	/* 0x24 */
+/* Instructions for the JTAG mailbox */
+#define IR_TEST_REG		0x54	/* 0x2A */
+/* Instructions for the Configuration Fuse */
+#define IR_CONFIG_FUSES		0x94
+/* Bypass instruction */
+#define IR_BYPASS		0xFF	/* 0xFF */
+/* Instructions for the EEM */
+#define IR_EMEX_DATA_EXCHANGE	0x90 /* 0x09 */
+#define IR_EMEX_WRITE_CONTROL	0x30 /* 0x0C */
+#define IR_EMEX_READ_CONTROL	0xD0 /* 0x0B */
+
+/* EEM stuff */
+#define EEM_STOPPED 0x0080
+#define EEM_EN      0x0001
+#define CLEAR_STOP  0x0002
+#define EMU_CLK_EN  0x0004
+
+#define CNTRL_SIG_CPUSUSP (1<<8)
+
+#define jtag_tms_set(p)		p->f->jtdev_tms(p, 1)
+#define jtag_tms_clr(p)		p->f->jtdev_tms(p, 0)
+#define jtag_tck_set(p)		p->f->jtdev_tck(p, 1)
+#define jtag_tck_clr(p)		p->f->jtdev_tck(p, 0)
+#define jtag_tdi_set(p)		p->f->jtdev_tdi(p, 1)
+#define jtag_tdi_clr(p)		p->f->jtdev_tdi(p, 0)
+#define jtag_tclk_set(p)	p->f->jtdev_tclk(p, 1)
+#define jtag_tclk_clr(p)	p->f->jtdev_tclk(p, 0)
+#define jtag_rst_set(p)		p->f->jtdev_rst(p, 1)
+#define jtag_rst_clr(p)		p->f->jtdev_rst(p, 0)
+#define jtag_tst_set(p)		p->f->jtdev_tst(p, 1)
+#define jtag_tst_clr(p)		p->f->jtdev_tst(p, 0)
+
+#define jtag_led_green_on(p)	p->f->jtdev_led_green(p, 1)
+#define jtag_led_green_off(p)	p->f->jtdev_led_green(p, 0)
+#define jtag_led_red_on(p)	p->f->jtdev_led_red(p, 1)
+#define jtag_led_red_off(p)	p->f->jtdev_led_red(p, 0)
+
+#define jtag_ir_shift(p, ir) p->f->jtdev_ir_shift(p, ir)
+#define jtag_dr_shift_8(p, dr) p->f->jtdev_dr_shift_8(p, dr)
+#define jtag_dr_shift_16(p, dr) p->f->jtdev_dr_shift_16(p, dr)
+#define jtag_dr_shift_20(p, dr) p->f->jtdev_dr_shift_20(p, dr)
+#define jtag_tms_sequence(p, bits, tms) p->f->jtdev_tms_sequence(p, bits, tms)
+#define jtag_init_dap(p) p->f->jtdev_init_dap(p)
+
+

drivers/jtdev.c

@@ -359,6 +359,7 @@ const struct jtdev_func jtdev_func_pif = {
   .jtdev_ir_shift    = jtag_default_ir_shift,
   .jtdev_dr_shift_8  = jtag_default_dr_shift_8,
   .jtdev_dr_shift_16 = jtag_default_dr_shift_16,
+  .jtdev_dr_shift_20 = jtag_default_dr_shift_20,
   .jtdev_tms_sequence= jtag_default_tms_sequence,
   .jtdev_init_dap    = jtag_default_init_dap
 };

drivers/jtdev.h

@@ -22,16 +22,25 @@
 
 #include <stdint.h>
 
+#include "device.h"
+#include "jtaglib_defs.h"
+
 struct jtdev_func;
 struct jtdev {
+	struct device base;
 	int		port;
 	uint8_t		data_register;
 	uint8_t		control_register;
+	uint8_t		jtag_id;
+	uint8_t		cpu_type;
 	int		failed;
 	const struct jtdev_func * f;
+
+	address_t regs[DEVICE_NUM_REGS];
+	uint16_t wdtctl;
 };
 
-struct jtdev_func{
+struct jtdev_func {
 /* Initialize/destroy a parallel-port JTAG interface. jtdev_open()
  * returns 0 on success or -1 if an error occurs.
  *
@@ -68,6 +77,7 @@ struct jtdev_func{
 	uint8_t (*jtdev_ir_shift)(struct jtdev *p, uint8_t ir);
 	uint8_t (*jtdev_dr_shift_8)(struct jtdev *p, uint8_t dr);
 	uint16_t (*jtdev_dr_shift_16)(struct jtdev *p, uint16_t dr);
+	uint32_t (*jtdev_dr_shift_20)(struct jtdev *p, uint32_t dr);
 	void (*jtdev_tms_sequence)(struct jtdev *p, int bits, unsigned int value);
 	void (*jtdev_init_dap)(struct jtdev *p);
 };

drivers/jtdev_bus_pirate.c

@@ -357,6 +357,7 @@ const struct jtdev_func jtdev_func_bp = {
   .jtdev_ir_shift    = jtag_default_ir_shift,
   .jtdev_dr_shift_8  = jtag_default_dr_shift_8,
   .jtdev_dr_shift_16 = jtag_default_dr_shift_16,
+  .jtdev_dr_shift_20 = jtag_default_dr_shift_20,
   .jtdev_tms_sequence= jtag_default_tms_sequence,
   .jtdev_init_dap    = jtag_default_init_dap
 };

drivers/jtdev_gpio.c

@@ -268,6 +268,7 @@ const struct jtdev_func jtdev_func_gpio = {
   .jtdev_ir_shift    = jtag_default_ir_shift,
   .jtdev_dr_shift_8  = jtag_default_dr_shift_8,
   .jtdev_dr_shift_16 = jtag_default_dr_shift_16,
+  .jtdev_dr_shift_20 = jtag_default_dr_shift_20,
   .jtdev_tms_sequence= jtag_default_tms_sequence,
   .jtdev_init_dap    = jtag_default_init_dap
 };

drivers/mehfet.c

@@ -32,7 +32,6 @@
 
 
 struct mehfet_device {
-	struct device base;
 	struct jtdev jtag;
 	transport_t trans;
 	enum mehfet_conn connstat;
@@ -157,6 +156,17 @@ static uint16_t jtmf_dr_shift_16(struct jtdev *p, uint16_t dr)
 
 	return outbuf[0] | ((uint16_t)outbuf[1] << 8);
 }
+static uint32_t jtmf_dr_shift_20(struct jtdev *p, uint32_t dr)
+{
+	struct mehfet_device* d = JTMF_GET_DEV(p);
+
+	uint8_t inbuf[3] = { dr & 0xff, (dr >> 8) & 0xff, (dr >> 16) & 0x0f };
+	uint8_t outbuf[3];
+	int r = mehfet_cmd_drshift(d->trans, 20, inbuf, outbuf);
+	if (r < 0) p->failed = 1;
+
+	return outbuf[0] | ((uint32_t)outbuf[1] << 8) | ((uint32_t)outbuf[2] << 16);
+}
 static void jtmf_tms_sequence(struct jtdev *p, int bits, unsigned int value)
 {
 	struct mehfet_device* d = JTMF_GET_DEV(p);
@@ -207,262 +217,12 @@ static const struct jtdev_func jtdev_func_mehfet = {
 	.jtdev_ir_shift    = jtmf_ir_shift,
 	.jtdev_dr_shift_8  = jtmf_dr_shift_8,
 	.jtdev_dr_shift_16 = jtmf_dr_shift_16,
+	.jtdev_dr_shift_20 = jtmf_dr_shift_20,
 	.jtdev_tms_sequence= jtmf_tms_sequence,
 	.jtdev_init_dap    = jtmf_init_dap
 };
 
 
-/*---------------------------------------------------------------------------*/
-
-// TODO: these five are kinda copied from pif.c, should be deduplicated
-
-static int read_words(device_t dev_base, const struct chipinfo_memory *m,
-		address_t addr, address_t len, uint8_t *data)
-{
-#ifdef DEBUG_MEHFET_DRIVER
-	printc_dbg("mehfet: read_words: addr=0x%04x, len=0x%x\n", addr, len);
-#endif
-
-	struct mehfet_device* dev = (struct mehfet_device*)dev_base;
-	struct jtdev *p = &dev->jtag;
-
-	for (unsigned int index = 0; index < len; index += 2) {
-		unsigned int word = jtag_read_mem(p, 16, addr+index);
-		data[index  ] =  word       & 0x00ff;
-		data[index+1] = (word >> 8) & 0x00ff;
-	}
-
-	return p->failed ? -1 : len;
-}
-
-static int write_ram_word(struct jtdev *p, address_t addr, uint16_t value)
-{
-	jtag_write_mem(p, 16, addr, value);
-
-	return p->failed ? -1 : 0;
-}
-
-static int write_flash_block(struct jtdev *p, address_t addr,
-		address_t len, const uint8_t *data)
-{
-	uint16_t* word = malloc( len / 2 * sizeof(*word) );
-	if (!word) {
-		pr_error("mehfet: failed to allocate memory");
-		return -1;
-	}
-
-	for (unsigned int i = 0; i < len/2; i++) {
-		word[i]=data[2*i] + (((uint16_t)data[2*i+1]) << 8);
-	}
-	jtag_write_flash(p, addr, len/2, word);
-
-	free(word);
-
-	return p->failed ? -1 : 0;
-}
-
-/* Write a word-aligned block to any kind of memory.
- * returns the number of bytes written or -1 on failure
- */
-static int write_words(device_t dev_base, const struct chipinfo_memory *m,
-		address_t addr, address_t len, const uint8_t *data)
-{
-	struct mehfet_device* dev = (struct mehfet_device*)dev_base;
-	struct jtdev *p = &dev->jtag;
-	int r;
-
-	if (m->type != CHIPINFO_MEMTYPE_FLASH) {
-#ifdef DEBUG_MEHFET_DRIVER
-		printc_dbg("mehfet: write_words: addr=0x%04x, len=0x%x data=0x%04x\n",
-				addr, len, r16le(data));
-		if (len != 2) {
-			printc_dbg("mehfet: WARN: write_words: len != 2! but 0x%04x\n", len);
-			__builtin_trap();
-		}
-#endif
-		len = 2;
-		r = write_ram_word(p, addr, r16le(data));
-	} else {
-#ifdef DEBUG_MEHFET_DRIVER
-		printc_dbg("mehfet: write_flash_block: addr=0x%04x, len=0x%x\n", addr, len);
-#endif
-		r = write_flash_block(p, addr, len, data);
-	}
-
-	if (r < 0) {
-		printc_err("mehfet: write_words at address 0x%x failed\n", addr);
-		return -1;
-	}
-
-	return len;
-}
-
-/*---------------------------------------------------------------------------*/
-
-static int mehfet_readmem(device_t dev_base, address_t addr,
-			   uint8_t *mem, address_t len)
-{
-	struct mehfet_device* dev = (struct mehfet_device*)dev_base;
-	dev->jtag.failed = 0;
-	return readmem(dev_base, addr, mem, len, read_words);
-}
-
-static int mehfet_writemem(device_t dev_base, address_t addr,
-			    const uint8_t *mem, address_t len)
-{
-	struct mehfet_device* dev = (struct mehfet_device*)dev_base;
-	dev->jtag.failed = 0;
-	return writemem(dev_base, addr, mem, len, write_words, read_words);
-}
-
-static int mehfet_setregs(device_t dev_base, const address_t *regs)
-{
-	struct mehfet_device* dev = (struct mehfet_device*)dev_base;
-
-	dev->jtag.failed = 0;
-
-#ifdef DEBUG_MEHFET_DRIVER
-	printc_dbg("mehfet: set regs\n");
-#endif
-	for (int i = 0; i < DEVICE_NUM_REGS; i++) {
-#ifdef DEBUG_MEHFET_DRIVER
-		printc_dbg("mehfet:  [%d] = 0x%04x\n", i, regs[i]);
-#endif
-		jtag_write_reg(&dev->jtag, i, regs[i]);
-	}
-	return dev->jtag.failed ? -1 : 0;
-}
-
-static int mehfet_getregs(device_t dev_base, address_t *regs)
-{
-	struct mehfet_device* dev = (struct mehfet_device*)dev_base;
-
-	dev->jtag.failed = 0;
-
-#ifdef DEBUG_MEHFET_DRIVER
-	printc_dbg("mehfet: get regs\n");
-#endif
-	for (int i = 0; i < DEVICE_NUM_REGS; i++) {
-		regs[i] = jtag_read_reg(&dev->jtag, i);
-#ifdef DEBUG_MEHFET_DRIVER
-		printc_dbg("mehfet:  [%d] = 0x%04x\n", i, regs[i]);
-#endif
-	}
-
-	return dev->jtag.failed ? -1 : 0;
-}
-
-static int mehfet_ctl(device_t dev_base, device_ctl_t type)
-{
-	struct mehfet_device* dev = (struct mehfet_device*)dev_base;
-
-	dev->jtag.failed = 0;
-
-	switch (type) {
-	case DEVICE_CTL_RESET:
-		/* perform soft reset */
-#ifdef DEBUG_MEHFET_DRIVER
-		printc_dbg("mehfet: soft reset (PUC)\n");
-#endif
-		jtag_execute_puc(&dev->jtag);
-		break;
-
-	case DEVICE_CTL_RUN:
-#ifdef DEBUG_MEHFET_DRIVER
-		printc_dbg("mehfet: set breakpoints\n");
-#endif
-		/* transfer changed breakpoints to device */
-		if (jtag_refresh_bps("mehfet", &dev->base, &dev->jtag) < 0) return -1;
-
-#ifdef DEBUG_MEHFET_DRIVER
-		printc_dbg("mehfet: run @ current PC\n");
-#endif
-		/* start program execution at current PC */
-		jtag_release_device(&dev->jtag, 0xffff);
-		break;
-
-	case DEVICE_CTL_HALT:
-#ifdef DEBUG_MEHFET_DRIVER
-		printc_dbg("mehfet: halt\n");
-#endif
-		/* take device under JTAG control */
-		jtag_get_device(&dev->jtag);
-		break;
-
-	case DEVICE_CTL_STEP:
-#ifdef DEBUG_MEHFET_DRIVER
-		printc_dbg("mehfet: single-step\n");
-#endif
-		/* execute next instruction at current PC */
-		jtag_single_step(&dev->jtag);
-		break;
-
-	default:
-		printc_err("mehfet: unsupported operation %d\n", type);
-		return -1;
-	}
-
-	return dev->jtag.failed ? -1 : 0;
-}
-
-static device_status_t mehfet_poll(device_t dev_base)
-{
-	struct mehfet_device* dev = (struct mehfet_device*)dev_base;
-
-	if (delay_ms(100) < 0 || ctrlc_check())
-		return DEVICE_STATUS_INTR;
-
-	int r = jtag_cpu_state(&dev->jtag);
-
-#ifdef DEBUG_MEHFET_DRIVER
-	printc_dbg("mehfet: cpu state: %d\n", r);
-#endif
-
-	if (r == 1) return DEVICE_STATUS_HALTED;
-
-	return DEVICE_STATUS_RUNNING;
-}
-
-static int mehfet_erase(device_t dev_base, device_erase_type_t type,
-			 address_t addr)
-{
-	struct mehfet_device* dev = (struct mehfet_device*)dev_base;
-
-	dev->jtag.failed = 0;
-
-	switch (type) {
-	case DEVICE_ERASE_MAIN:
-		jtag_erase_flash(&dev->jtag, JTAG_ERASE_MAIN, addr);
-		break;
-	case DEVICE_ERASE_ALL:
-		jtag_erase_flash(&dev->jtag, JTAG_ERASE_MASS, addr);
-		break;
-	case DEVICE_ERASE_SEGMENT:
-		jtag_erase_flash(&dev->jtag, JTAG_ERASE_SGMT, addr);
-		break;
-	default: return -1;
-	}
-
-#ifdef DEBUG_MEHFET_DRIVER
-	printc_dbg("mehfet: erase flash %d at %04x: %s\n", type, addr, dev->jtag.failed ? "failed" : "succeeded");
-#endif
-
-	return dev->jtag.failed ? -1 : 0;
-}
-
-static int mehfet_getconfigfuses(device_t dev_base)
-{
-	struct mehfet_device* dev = (struct mehfet_device*)dev_base;
-
-	int r = jtag_get_config_fuses(&dev->jtag);
-
-#ifdef DEBUG_MEHFET_DRIVER
-	printc_dbg("mehfet: get_config_fuses: %d\n", r);
-#endif
-
-	return r;
-}
-
 /*---------------------------------------------------------------------------*/
 
 static int check_dev_ok(struct mehfet_device* dev, const struct device_args* args,
@@ -518,32 +278,6 @@ static int check_dev_ok(struct mehfet_device* dev, const struct device_args* arg
 	return 0;
 }
 
-static int init_device(struct mehfet_device* dev) {
-	printc_dbg("Starting JTAG\n");
-
-	unsigned int jtagid = jtag_init(&dev->jtag);
-	if (dev->jtag.failed) return -1;
-
-	printc("JTAG ID: 0x%02x\n", jtagid);
-	if (jtagid != 0x89 && jtagid != 0x91) {
-		printc_err("mehfet: unexpected JTAG ID: 0x%02x\n", jtagid);
-		jtag_release_device(&dev->jtag, 0xfffe);
-		return -1;
-	}
-
-	// JTAG fuse check has been performed, so we can now switch to a
-	// higher-speed physical transport suitable for ~350 kHz TCLK strobes used
-	// in (and required for) flash programming
-	int r = mehfet_cmd_set_clkspeed(dev->trans, true);
-	if (r < 0) {
-		jtag_release_device(&dev->jtag, 0xfffe);
-		return -1;
-	}
-
-	return 0;
-}
-
-
 static void mehfet_destroy(device_t dev_base) {
 	struct mehfet_device* dev = (struct mehfet_device*)dev_base;
 
@@ -580,9 +314,9 @@ static device_t mehfet_open(const struct device_args* args) {
 		return NULL;
 	}
 
-	dev->base.type = &device_mehfet;
-	dev->base.max_breakpoints = 2; // TODO
-	dev->base.need_probe = 1; // TODO
+	dev->jtag.base.type = &device_mehfet;
+	dev->jtag.base.max_breakpoints = 2; // TODO
+	dev->jtag.base.need_probe = 1; // TODO
 	dev->jtag.f = &jtdev_func_mehfet;
 
 	// the MehFET currently doesn't have a designated PID, so we can't really
@@ -608,7 +342,13 @@ static device_t mehfet_open(const struct device_args* args) {
 		goto FAIL;
 	}
 
-	r = init_device(dev);
+	r = jtag_dev_init(&dev->jtag);
+	if (r < 0) goto FAIL;
+
+	// JTAG fuse check has been performed, so we can now switch to a
+	// higher-speed physical transport suitable for ~350 kHz TCLK strobes used
+	// in (and required for) flash programming
+	r = mehfet_cmd_set_clkspeed(dev->trans, true);
 	if (r < 0) goto FAIL;
 
 #ifdef DEBUG_MEHFET_DRIVER
@@ -627,13 +367,13 @@ const struct device_class device_mehfet = {
 	.help = "MehFET USB JTAG/SBW device",
 	.open     = mehfet_open,
 	.destroy  = mehfet_destroy,
-	.readmem  = mehfet_readmem,
-	.writemem = mehfet_writemem,
-	.getregs  = mehfet_getregs,
-	.setregs  = mehfet_setregs,
-	.ctl      = mehfet_ctl,
-	.poll     = mehfet_poll,
-	.erase    = mehfet_erase,
-	.getconfigfuses = mehfet_getconfigfuses
+	.readmem  = jtag_dev_readmem,
+	.writemem = jtag_dev_writemem,
+	.getregs  = jtag_dev_getregs,
+	.setregs  = jtag_dev_setregs,
+	.ctl      = jtag_dev_ctl,
+	.poll     = jtag_dev_poll,
+	.erase    = jtag_dev_erase,
+	.getconfigfuses = jtag_dev_getconfigfuses
 };
 

drivers/mehfet_proto.c

@@ -402,6 +402,72 @@ static uint8_t bitswap(uint8_t in) {
 	return (bitswap_nyb(in&0xf) << 4) | bitswap_nyb(in>>4);
 }
 
+#ifdef DEBUG_MEHFET_PROTO_DRIVER
+static const char* get_ir_name(uint8_t ir) {
+	switch (ir) {
+	case 0x09: return "EMEX_DATA_EXCHANGE";
+	case 0x0a: return "EMEX_READ_TRIGGER";
+	case 0x0b: return "EMEX_READ_CONTROL";
+	case 0x0c: return "EMEX_WRITE_CONTROL";
+	case 0x0d: return "EMEX_READ_CONTROL_2";
+	case 0x0e: return "EMEX_WRITE_CONTROL_2";
+
+	case 0x11: return "CNTRL_SIG_HIGH_BYTE";
+	case 0x12: return "CNTRL_SIG_LOW_BYTE";
+	case 0x13: return "CNTRL_SIG_16BIT";
+	case 0x14: return "CNTRL_SIG_CAPTURE";
+	case 0x15: return "CNTRL_SIG_RELEASE";
+	case 0x17: return "COREIP_ID";
+
+	case 0x19: return "FLASH_16BIT_UPDATE";
+	case 0x1a: return "FLASH_CAPTURE";
+	case 0x1b: return "FLASH_16BIT_IN";
+	case 0x1c: return "FLASH_UPDATE";
+
+	case 0x21: return "CNTRL";
+	case 0x22: return "PREPARE_BLOW";
+	case 0x24: return "EX_BLOX";
+	case 0x29: return "CONFIG_FUSES";
+	case 0x2a: return "TEST_REG";
+	case 0x2f: return "TEST_3V_REG";
+
+	case 0x31: return "DUAL_8BIT";
+	case 0x32: return "DUAL_CAPTURE";
+	case 0x33: return "SELECT_MAIN";
+	case 0x34: return "SELECT_ESP";
+
+	case 0x41: return "DATA_16BIT";
+	case 0x42: return "DATA_CAPTURE";
+	case 0x43: return "DATA_QUICK";
+	case 0x44: return "DATA_PSA";
+	case 0x45: return "DATA_16BIT_OPT";
+	case 0x46: return "SHIFT_OUT_PSA";
+	case 0x47: return "DTA";
+
+	case 0x59: return "ACCEPT_KEY";
+
+	case 0x61: return "JMB_EXCHANGE";
+	case 0x62: return "JSTATE_ID";
+	case 0x64: return "TDO_EVENT";
+	case 0x65: return "TDO_EVENT_CTL";
+
+	case 0x81: return "ADDR_HIGH_BYTE";
+	case 0x82: return "ADDR_LOW_BYTE";
+	case 0x83: return "ADDR_16BIT";
+	case 0x84: return "ADDR_CAPTURE";
+
+	case 0x85: return "DATA_TO_ADDR";
+	case 0x86: return "CAPTURE_CPU_REG";
+	case 0x87: return "DEVICE_ID";
+	case 0x88: return "JMB_WRITE_32BIT_MODE";
+
+	case 0xFF: return "BYPASS";
+
+	default: return NULL;
+	}
+}
+#endif
+
 int mehfet_cmd_irshift(transport_t t, uint8_t newir, uint8_t* oldir) {
 	if (!oldir) return -1;
 
@@ -431,7 +497,12 @@ int mehfet_cmd_irshift(transport_t t, uint8_t newir, uint8_t* oldir) {
 	*oldir = buf[0];
 
 #ifdef DEBUG_MEHFET_PROTO_DRIVER
-	printc_dbg("mehfet: IRshift(new=0x%02x) = 0x%02x\n", newir, *oldir);
+	const char* s = get_ir_name(newir);
+	if (s) {
+		printc_dbg("mehfet: IRshift(new=IR_%s) = 0x%02x\n", s, *oldir);
+	} else {
+		printc_dbg("mehfet: IRshift(new=0x%02x) = 0x%02x\n", newir, *oldir);
+	}
 #endif
 
 	return 0;
@@ -463,9 +534,26 @@ int mehfet_cmd_drshift(transport_t t, uint32_t nbits, const uint8_t* newdr, uint
 	memcpy(olddr, buf, nbytes);
 
 #ifdef DEBUG_MEHFET_PROTO_DRIVER
-	printc_dbg("mehfet: DRshift(nbits=%u):\n", nbits);
-	debug_hexdump("\tin ", newdr, nbytes);
-	debug_hexdump("\tout", olddr, nbytes);
+	if (nbits == 16) {
+		uint16_t in, out;
+		in = newdr[0] | ((uint16_t)newdr[1] << 8);
+		out= olddr[0] | ((uint16_t)olddr[1] << 8);
+		printc_dbg("mehfet: DRshift(nbits=%u): in: %04x, out: %04x\n", nbits, in, out);
+	} else if (nbits == 20) {
+		uint32_t in, out;
+		in = newdr[0] | ((uint32_t)newdr[1] << 8) | ((uint32_t)newdr[2] << 16);
+		out= olddr[0] | ((uint32_t)olddr[1] << 8) | ((uint32_t)olddr[2] << 16);
+		printc_dbg("mehfet: DRshift(nbits=%u): in: %05x, out: %05x\n", nbits, in, out);
+	} else if (nbits == 32) {
+		uint32_t in, out;
+		in = newdr[0] | ((uint32_t)newdr[1] << 8) | ((uint32_t)newdr[2] << 16) | ((uint32_t)newdr[3] << 24);
+		out= olddr[0] | ((uint32_t)olddr[1] << 8) | ((uint32_t)olddr[2] << 16) | ((uint32_t)olddr[3] << 24);
+		printc_dbg("mehfet: DRshift(nbits=%u): in: %08x, out: %08x\n", nbits, in, out);
+	} else {
+		printc_dbg("mehfet: DRshift(nbits=%u):\n", nbits);
+		debug_hexdump("\tin ", newdr, nbytes);
+		debug_hexdump("\tout", olddr, nbytes);
+	}
 #endif
 
 	return 0;

drivers/pif.c

@@ -35,256 +35,10 @@
 #include "ctrlc.h"
 
 struct pif_device {
-  struct device		base;
   struct jtdev		jtag;
 };
 
-/*============================================================================*/
-/* pif MSP430 JTAG operations */
-
 /*----------------------------------------------------------------------------*/
-/* Read a word-aligned block from any kind of memory
- * returns the number of bytes read or -1 on failure
- */
-static int read_words(device_t dev, const struct chipinfo_memory *m,
-		address_t addr, address_t len, uint8_t *data)
-{
-  struct pif_device *pif = (struct pif_device *)dev;
-  struct jtdev *p = &pif->jtag;
-  unsigned int index;
-  unsigned int word;
-
-  for ( index = 0; index < len; index += 2 ) {
-    word = jtag_read_mem( p, 16, addr+index );
-    data[index]   =  word       & 0x00ff;
-    data[index+1] = (word >> 8) & 0x00ff;
-  }
-
-  return p->failed ? -1 : len;
-}
-
-/*----------------------------------------------------------------------------*/
-/* Write a word to RAM */
-static int write_ram_word( struct jtdev *p, address_t addr,
-		    uint16_t  value )
-{
-  jtag_write_mem( p, 16, addr, value );
-
-  return p->failed ? -1 : 0;
-}
-
-/*----------------------------------------------------------------------------*/
-/* Write a word-aligned flash block. */
-/* The starting address must be within the flash memory range. */
-
-static int write_flash_block( struct jtdev *p, address_t addr,
-			      address_t len,
-			      const uint8_t *data)
-{
-  unsigned int i;
-  uint16_t *word;
-
-  word = malloc( len / 2 * sizeof(*word) );
-  if (!word) {
-	pr_error("pif: failed to allocate memory");
-	return -1;
-  }
-
-  for(i = 0; i < len/2; i++) {
-    word[i]=data[2*i] + (((uint16_t)data[2*i+1]) << 8);
-  }
-  jtag_write_flash( p, addr, len/2, word );
-
-  free(word);
-
-  return p->failed ? -1 : 0;
-}
-
-/* Write a word-aligned block to any kind of memory.
- * returns the number of bytes written or -1 on failure
- */
-static int write_words(device_t dev, const struct chipinfo_memory *m,
-		address_t addr, address_t len, const uint8_t *data)
-{
-  struct pif_device *pif = (struct pif_device *)dev;
-  struct jtdev *p = &pif->jtag;
-  int r;
-
-  if (m->type != CHIPINFO_MEMTYPE_FLASH) {
-    len = 2;
-    r = write_ram_word(p, addr, r16le(data));
-  } else {
-    r = write_flash_block(p, addr, len, data);
-  }
-
-  if (r < 0) {
-    printc_err("pif: write_words at address 0x%x failed\n", addr);
-    return -1;
-  }
-
-  return len;
-}
-
-/*----------------------------------------------------------------------------*/
-static int init_device(struct jtdev *p)
-{
-  unsigned int jtag_id;
-
-  printc_dbg("Starting JTAG\n");
-  jtag_id = jtag_init(p);
-  printc("JTAG ID: 0x%02x\n", jtag_id);
-  if (jtag_id != 0x89 && jtag_id != 0x91) {
-    printc_err("pif: unexpected JTAG ID: 0x%02x\n", jtag_id);
-    jtag_release_device(p, 0xfffe);
-    return -1;
-  }
-
-  return 0;
-}
-
-/*===== MSPDebug Device interface ============================================*/
-
-/*----------------------------------------------------------------------------*/
-static int pif_readmem( device_t  dev_base,
-			address_t addr,
-			uint8_t*  mem,
-			address_t len )
-{
-  struct pif_device *dev = (struct pif_device *)dev_base;
-  dev->jtag.failed = 0;
-  return readmem(dev_base, addr, mem, len, read_words);
-}
-
-/*----------------------------------------------------------------------------*/
-static int pif_writemem( device_t       dev_base,
-			 address_t      addr,
-			 const uint8_t* mem,
-			 address_t      len )
-{
-  struct pif_device *dev = (struct pif_device *)dev_base;
-  dev->jtag.failed = 0;
-  return writemem(dev_base, addr, mem, len, write_words, read_words);
-}
-
-/*----------------------------------------------------------------------------*/
-static int pif_getregs(device_t dev_base, address_t *regs)
-{
-  struct pif_device *dev = (struct pif_device *)dev_base;
-  int i;
-
-  dev->jtag.failed = 0;
-
-  for (i = 0; i < DEVICE_NUM_REGS; i++)
-    regs[i] = jtag_read_reg(&dev->jtag, i);
-
-  return dev->jtag.failed ? -1 : 0;
-}
-
-/*----------------------------------------------------------------------------*/
-static int pif_setregs( device_t dev_base, const address_t* regs )
-{
-  struct pif_device *dev = (struct pif_device *)dev_base;
-  int i;
-
-  dev->jtag.failed = 0;
-
-  for (i = 0; i < DEVICE_NUM_REGS; i++) {
-    jtag_write_reg( &dev->jtag, i, regs[i] );
-  }
-  return dev->jtag.failed ? -1 : 0;
-}
-
-/*----------------------------------------------------------------------------*/
-static int pif_ctl(device_t dev_base, device_ctl_t type)
-{
-  struct pif_device *dev = (struct pif_device *)dev_base;
-
-  dev->jtag.failed = 0;
-
-  switch (type) {
-    case DEVICE_CTL_RESET:
-      /* perform soft reset */
-      jtag_execute_puc(&dev->jtag);
-      break;
-
-    case DEVICE_CTL_RUN:
-      /* transfer changed breakpoints to device */
-      if (jtag_refresh_bps("pif", &dev->base, &dev->jtag) < 0) {
-	return -1;
-      }
-      /* start program execution at current PC */
-      jtag_release_device(&dev->jtag, 0xffff);
-      break;
-
-    case DEVICE_CTL_HALT:
-      /* take device under JTAG control */
-      jtag_get_device(&dev->jtag);
-      break;
-
-    case DEVICE_CTL_STEP:
-      /* execute next instruction at current PC */
-      jtag_single_step(&dev->jtag);
-      break;
-
-    default:
-      printc_err("pif: unsupported operation\n");
-      return -1;
-  }
-
-  return dev->jtag.failed ? -1 : 0;
-}
-
-/*----------------------------------------------------------------------------*/
-static device_status_t pif_poll(device_t dev_base)
-{
-  struct pif_device *dev = (struct pif_device *)dev_base;
-
-  if (delay_ms(100) < 0 || ctrlc_check())
-    return DEVICE_STATUS_INTR;
-
-  if (jtag_cpu_state(&dev->jtag) == 1) {
-    return DEVICE_STATUS_HALTED;
-  }
-
-  return DEVICE_STATUS_RUNNING;
-}
-
-/*----------------------------------------------------------------------------*/
-static int pif_erase( device_t dev_base,
-		      device_erase_type_t type,
-		      address_t addr )
-{
-  struct pif_device *dev = (struct pif_device *)dev_base;
-
-  dev->jtag.failed = 0;
-
-  switch(type) {
-    case DEVICE_ERASE_MAIN:
-      jtag_erase_flash ( &dev->jtag, JTAG_ERASE_MAIN, addr );
-      break;
-    case DEVICE_ERASE_ALL:
-      jtag_erase_flash ( &dev->jtag, JTAG_ERASE_MASS, addr );
-      break;
-    case DEVICE_ERASE_SEGMENT:
-      jtag_erase_flash ( &dev->jtag, JTAG_ERASE_SGMT, addr );
-      break;
-    default:
-      return -1;
-  }
-
-  return dev->jtag.failed ? -1 : 0;
-}
-
-/*----------------------------------------------------------------------------*/
-static int pif_getconfigfuses(device_t dev_base)
-{
-  struct pif_device *dev = (struct pif_device *)dev_base;
-
-  return jtag_get_config_fuses(&dev->jtag);
-}
-
-/*----------------------------------------------------------------------------*/
-
 
 static device_t pif_open(const struct device_args *args)
 {
@@ -307,9 +61,9 @@ static device_t pif_open(const struct device_args *args)
   }
 
   memset(dev, 0, sizeof(*dev));
-  dev->base.type = &device_pif;
-  dev->base.max_breakpoints = 2; //supported by all devices
-  dev->base.need_probe = 1;
+  dev->jtag.base.type = &device_pif;
+  dev->jtag.base.max_breakpoints = 2; //supported by all devices
+  dev->jtag.base.need_probe = 1;
   (&dev->jtag)->f = &jtdev_func_pif;
 
   if ((&dev->jtag)->f->jtdev_open(&dev->jtag, args->path) < 0) {
@@ -318,13 +72,13 @@ static device_t pif_open(const struct device_args *args)
     return NULL;
   }
 
-  if (init_device(&dev->jtag) < 0) {
+  if (jtag_dev_init(&dev->jtag) < 0) {
     printc_err("pif: initialization failed\n");
     free(dev);
     return NULL;
   }
 
-  return &dev->base;
+  return &dev->jtag.base;
 }
 
 /*----------------------------------------------------------------------------*/
@@ -351,8 +105,8 @@ static device_t gpio_open(const struct device_args *args)
   }
 
   memset(dev, 0, sizeof(*dev));
-  dev->base.type = &device_pif;
-  dev->base.max_breakpoints = 0;
+  dev->jtag.base.type = &device_pif;
+  dev->jtag.base.max_breakpoints = 0;
   (&dev->jtag)->f = &jtdev_func_gpio;
 
   if ((&dev->jtag)->f->jtdev_open(&dev->jtag, args->path) < 0) {
@@ -361,13 +115,13 @@ static device_t gpio_open(const struct device_args *args)
     return NULL;
   }
 
-  if (init_device(&dev->jtag) < 0) {
+  if (jtag_dev_init(&dev->jtag) < 0) {
     printc_err("gpio: initialization failed\n");
     free(dev);
     return NULL;
   }
 
-  return &dev->base;
+  return &dev->jtag.base;
 }
 
 /*----------------------------------------------------------------------------*/
@@ -394,9 +148,9 @@ static device_t bp_open(const struct device_args *args)
   }
 
   memset(dev, 0, sizeof(*dev));
-  dev->base.type = &device_pif;
-  dev->base.max_breakpoints = 2; //supported by all devices
-  dev->base.need_probe = 1;
+  dev->jtag.base.type = &device_pif;
+  dev->jtag.base.max_breakpoints = 2; //supported by all devices
+  dev->jtag.base.need_probe = 1;
   (&dev->jtag)->f = &jtdev_func_bp;
 
   if ((&dev->jtag)->f->jtdev_open(&dev->jtag, args->path) < 0) {
@@ -405,13 +159,13 @@ static device_t bp_open(const struct device_args *args)
     return NULL;
   }
 
-  if (init_device(&dev->jtag) < 0) {
+  if (jtag_dev_init(&dev->jtag) < 0) {
     printc_err("bp: initialization failed\n");
     free(dev);
     return NULL;
   }
 
-  return &dev->base;
+  return &dev->jtag.base;
 }
 
 /*----------------------------------------------------------------------------*/
@@ -432,14 +186,14 @@ const struct device_class device_pif = {
   .help     = "Parallel Port JTAG",
   .open     = pif_open,
   .destroy  = pif_destroy,
-  .readmem  = pif_readmem,
-  .writemem = pif_writemem,
-  .getregs  = pif_getregs,
-  .setregs  = pif_setregs,
-  .ctl      = pif_ctl,
-  .poll     = pif_poll,
-  .erase    = pif_erase,
-  .getconfigfuses = pif_getconfigfuses
+  .readmem  = jtag_dev_readmem,
+  .writemem = jtag_dev_writemem,
+  .getregs  = jtag_dev_getregs,
+  .setregs  = jtag_dev_setregs,
+  .ctl      = jtag_dev_ctl,
+  .poll     = jtag_dev_poll,
+  .erase    = jtag_dev_erase,
+  .getconfigfuses = jtag_dev_getconfigfuses
 };
 
 const struct device_class device_gpio = {
@@ -447,14 +201,14 @@ const struct device_class device_gpio = {
   .help     = "/sys/class/gpio direct connect",
   .open     = gpio_open,
   .destroy  = pif_destroy,
-  .readmem  = pif_readmem,
-  .writemem = pif_writemem,
-  .getregs  = pif_getregs,
-  .setregs  = pif_setregs,
-  .ctl      = pif_ctl,
-  .poll     = pif_poll,
-  .erase    = pif_erase,
-  .getconfigfuses = pif_getconfigfuses
+  .readmem  = jtag_dev_readmem,
+  .writemem = jtag_dev_writemem,
+  .getregs  = jtag_dev_getregs,
+  .setregs  = jtag_dev_setregs,
+  .ctl      = jtag_dev_ctl,
+  .poll     = jtag_dev_poll,
+  .erase    = jtag_dev_erase,
+  .getconfigfuses = jtag_dev_getconfigfuses
 };
 
 const struct device_class device_bp = {
@@ -462,12 +216,12 @@ const struct device_class device_bp = {
   .help     = "Bus Pirate JTAG, MISO-TDO, MOSI-TDI, CS-TMS, AUX-RESET, CLK-TCK",
   .open     = bp_open,
   .destroy  = pif_destroy,
-  .readmem  = pif_readmem,
-  .writemem = pif_writemem,
-  .getregs  = pif_getregs,
-  .setregs  = pif_setregs,
-  .ctl      = pif_ctl,
-  .poll     = pif_poll,
-  .erase    = pif_erase,
-  .getconfigfuses = pif_getconfigfuses
+  .readmem  = jtag_dev_readmem,
+  .writemem = jtag_dev_writemem,
+  .getregs  = jtag_dev_getregs,
+  .setregs  = jtag_dev_setregs,
+  .ctl      = jtag_dev_ctl,
+  .poll     = jtag_dev_poll,
+  .erase    = jtag_dev_erase,
+  .getconfigfuses = jtag_dev_getconfigfuses
 };

drivers/v3hil.c

@@ -25,7 +25,7 @@
 #include "opdb.h"
 
 #ifdef DEBUG_V3HIL
-#define dbg_printc(fmt, ...) printc_dbg("v3hil: " fmt, ##__VA_ARGS__)
+#define dbg_printc(fmt, ...) printc_dbg("v3hil: %s:%d: " fmt, __func__, __LINE__, ##__VA_ARGS__)
 #else
 #define dbg_printc(fmt, ...) do{}while(0)
 #endif
@@ -1121,8 +1121,11 @@ static int idproc_9x(struct v3hil *fet, uint32_t dev_id_ptr,
 		if (tag == 0xff)
 			break;
 
-		if ((tag == 0x14) && (len >= 2))
+		if ((tag == 0x14) && (len >= 2)) {
+			// FIXME: this looks like it's reading from the wrong address?
 			id->ver_sub_id = r16le(fet->hal.payload);
+			// FIXME: break here?
+		}
 
 		i += len;
 	}

ui/cmddb.c

@@ -190,11 +190,12 @@ const struct cmddb_record commands[] = {
 		.func = cmd_erase,
 		.help =
 "erase [all|segment] [address]\n"
-"erase segrange <address> <size> <seg-size>\n"
+"erase segrange <address> <total-size> <seg-size>\n"
 "    Erase the device under test. With no arguments, erases all of main\n"
 "    memory. Specify arguments to perform a mass erase, or to erase\n"
 "    individual segments. The \"segrange\" mode is used to erase an\n"
-"    address range via a series of segment erases.\n"
+"    address range via a series of segment erases, 'total-size' being"
+"    a multiple of 'seg-size'.\n"
 	},
 	{
 		.name = "step",

ui/devcmd.c

@@ -207,6 +207,12 @@ int cmd_erase(char **arg)
 					   "0x%x\n", segment_size);
 				return -1;
 			}
+
+			if (total_size % segment_size != 0) {
+				printc_err("erase: total size must be a multiple of the "
+				           "segment size!\n");
+				return -1;
+			}
 		} else {
 			printc_err("erase: unknown erase type: %s\n",
 				    type_text);