#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,
};